From 6564cfd5d1d44425335296eeab3cbea75ca69fbd Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 5 Jun 2017 18:33:32 -0700 Subject: [PATCH 01/42] WIP --- .../animation/src/AnimInverseKinematics.cpp | 138 ++++++++++++++---- .../animation/src/AnimInverseKinematics.h | 17 ++- libraries/animation/src/IKTarget.h | 10 +- 3 files changed, 126 insertions(+), 39 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 77437e79b9..41dae120fd 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -23,6 +23,25 @@ #include "CubicHermiteSpline.h" #include "AnimUtil.h" +static void lookupJointChainInfo(AnimInverseKinematics::JointChainInfo* jointChainInfos, size_t numJointChainInfos, + int indexA, int indexB, + AnimInverseKinematics::JointChainInfo** jointChainInfoA, + AnimInverseKinematics::JointChainInfo** jointChainInfoB) { + *jointChainInfoA = nullptr; + *jointChainInfoB = nullptr; + for (size_t i = 0; i < numJointChainInfos; i++) { + if (jointChainInfos[i].jointIndex == indexA) { + *jointChainInfoA = jointChainInfos + i; + } + if (jointChainInfos[i].jointIndex == indexB) { + *jointChainInfoB = jointChainInfos + i; + } + if (*jointChainInfoA && *jointChainInfoB) { + break; + } + } +} + AnimInverseKinematics::IKTargetVar::IKTargetVar(const QString& jointNameIn, const QString& positionVarIn, const QString& rotationVarIn, const QString& typeVarIn, const QString& weightVarIn, float weightIn, const std::vector& flexCoefficientsIn) : jointName(jointNameIn), @@ -148,6 +167,12 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: target.setWeight(weight); target.setFlexCoefficients(targetVar.numFlexCoefficients, targetVar.flexCoefficients); + // AJT: HACK REMOVE manually set pole vector. + if (targetVar.jointName == "RightHand") { + target.setPoleVector(glm::normalize(glm::vec3(-1, 0, 0))); + target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); + } + targets.push_back(target); if (targetVar.jointIndex > _maxTargetIndex) { @@ -298,7 +323,8 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const // the tip's parent-relative as we proceed up the chain glm::quat tipParentOrientation = absolutePoses[pivotIndex].rot(); - std::map debugJointMap; + const size_t MAX_CHAIN_DEPTH = 30; + JointChainInfo jointChainInfos[MAX_CHAIN_DEPTH]; // NOTE: if this code is removed, the head will remain rigid, causing the spine/hips to thrust forward backward // as the head is nodded. @@ -326,15 +352,8 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const } } - // store the relative rotation change in the accumulator - _rotationAccumulators[tipIndex].add(tipRelativeRotation, target.getWeight()); - glm::vec3 tipRelativeTranslation = _relativePoses[target.getIndex()].trans(); - _translationAccumulators[tipIndex].add(tipRelativeTranslation); - - if (debug) { - debugJointMap[tipIndex] = DebugJoint(tipRelativeRotation, tipRelativeTranslation, constrained); - } + jointChainInfos[chainDepth] = { tipRelativeRotation, tipRelativeTranslation, target.getWeight(), tipIndex, constrained }; } // cache tip absolute position @@ -344,6 +363,9 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const // descend toward root, pivoting each joint to get tip closer to target position while (pivotIndex != _hipsIndex && pivotsParentIndex != -1) { + + assert(chainDepth < MAX_CHAIN_DEPTH); + // compute the two lines that should be aligned glm::vec3 jointPosition = absolutePoses[pivotIndex].trans(); glm::vec3 leverArm = tipPosition - jointPosition; @@ -440,15 +462,8 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const } } - // store the relative rotation change in the accumulator - _rotationAccumulators[pivotIndex].add(newRot, target.getWeight()); - glm::vec3 newTrans = _relativePoses[pivotIndex].trans(); - _translationAccumulators[pivotIndex].add(newTrans); - - if (debug) { - debugJointMap[pivotIndex] = DebugJoint(newRot, newTrans, constrained); - } + jointChainInfos[chainDepth] = { newRot, newTrans, target.getWeight(), pivotIndex, constrained }; // keep track of tip's new transform as we descend towards root tipPosition = jointPosition + deltaRotation * (tipPosition - jointPosition); @@ -461,8 +476,73 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const chainDepth++; } + if (target.getPoleIndex() != -1) { + + int topIndex = target.getPoleIndex(); + int midIndex = _skeleton->getParentIndex(topIndex); + if (midIndex != -1) { + int baseIndex = _skeleton->getParentIndex(midIndex); + if (baseIndex != -1) { + int baseParentIndex = _skeleton->getParentIndex(baseIndex); + AnimPose topPose, midPose, basePose, baseParentPose; + AnimPose postAbsPoses[MAX_CHAIN_DEPTH]; + AnimPose accum = absolutePoses[_hipsIndex]; + for (int i = (int)chainDepth - 1; i >= 0; i--) { + accum = accum * AnimPose(glm::vec3(1.0f), jointChainInfos[i].relRot, jointChainInfos[i].relTrans); + postAbsPoses[i] = accum; + if (jointChainInfos[i].jointIndex == topIndex) { + topPose = accum; + } + if (jointChainInfos[i].jointIndex == midIndex) { + midPose = accum; + } + if (jointChainInfos[i].jointIndex == baseIndex) { + basePose = accum; + } + if (jointChainInfos[i].jointIndex == baseParentIndex) { + baseParentPose = accum; + } + } + + // AJT: TODO: check for parallel edge cases. + glm::vec3 d = glm::normalize(basePose.trans() - topPose.trans()); + glm::vec3 e = midPose.trans() - topPose.trans(); + glm::vec3 dHat = glm::dot(e, d) * d + topPose.trans(); + glm::vec3 eHat = glm::normalize(midPose.trans() - dHat); + float theta = acos(glm::dot(eHat, target.getPoleVector())); + glm::vec3 axis = glm::normalize(glm::cross(eHat, target.getPoleVector())); + glm::quat poleRot = glm::angleAxis(-theta, axis); + + if (debug) { + + 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); + + AnimPose geomToWorldPose = AnimPose(context.getRigToWorldMatrix() * context.getGeometryToRigMatrix()); + + const float POLE_VECTOR_LEN = 10.0f; + DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(basePose.trans()), geomToWorldPose.xformPoint(topPose.trans()), GREEN); + DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(dHat), geomToWorldPose.xformPoint(dHat + POLE_VECTOR_LEN * eHat), RED); + DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(dHat), geomToWorldPose.xformPoint(dHat + POLE_VECTOR_LEN * target.getPoleVector()), BLUE); + } + + glm::quat newBaseRelRot = glm::inverse(baseParentPose.rot()) * poleRot * basePose.rot() * glm::inverse(jointChainInfos[baseIndex].relRot); + jointChainInfos[baseIndex].relRot = glm::quat(); //newBaseRelRot * jointChainInfos[baseIndex].relRot; + + glm::quat newTopRelRot = glm::inverse(midPose.rot()) * glm::inverse(poleRot) * topPose.rot() * glm::inverse(jointChainInfos[topIndex].relRot); + jointChainInfos[topIndex].relRot = glm::quat(); //newTopRelRot * jointChainInfos[topIndex].relRot; + } + } + } + + for (size_t i = 0; i < chainDepth; i++) { + _rotationAccumulators[jointChainInfos[i].jointIndex].add(jointChainInfos[i].relRot, jointChainInfos[i].weight); + _translationAccumulators[jointChainInfos[i].jointIndex].add(jointChainInfos[i].relTrans, jointChainInfos[i].weight); + } + if (debug) { - debugDrawIKChain(debugJointMap, context); + debugDrawIKChain(jointChainInfos, chainDepth, context); } } @@ -1338,13 +1418,14 @@ void AnimInverseKinematics::debugDrawRelativePoses(const AnimContext& context) c } } -void AnimInverseKinematics::debugDrawIKChain(std::map& debugJointMap, const AnimContext& context) const { +void AnimInverseKinematics::debugDrawIKChain(JointChainInfo* jointChainInfos, size_t numJointChainInfos, 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; - poses[debugJoint.first].trans() = debugJoint.second.relTrans; + for (size_t i = 0; i < numJointChainInfos; i++) { + const JointChainInfo& info = jointChainInfos[i]; + poses[info.jointIndex].rot() = info.relRot; + poses[info.jointIndex].trans() = info.relTrans; } // convert relative poses to absolute @@ -1360,11 +1441,11 @@ void AnimInverseKinematics::debugDrawIKChain(std::map& debugJoi // 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()) { + int parentIndex = _skeleton->getParentIndex(i); + JointChainInfo* jointInfo = nullptr; + JointChainInfo* parentJointInfo = nullptr; + lookupJointChainInfo(jointChainInfos, numJointChainInfos, i, parentIndex, &jointInfo, &parentJointInfo); + if (jointInfo && parentJointInfo) { // transform local axes into world space. auto pose = poses[i]; @@ -1377,13 +1458,12 @@ void AnimInverseKinematics::debugDrawIKChain(std::map& debugJoi 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) { + if (parentJointInfo->constrained) { color = RED; } DebugDraw::getInstance().drawRay(pos, parentPos, color); diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index cc919c1684..2ed96ed6ed 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -26,6 +26,14 @@ class RotationConstraint; class AnimInverseKinematics : public AnimNode { public: + struct JointChainInfo { + glm::quat relRot; + glm::vec3 relTrans; + float weight; + int jointIndex; + bool constrained; + }; + explicit AnimInverseKinematics(const QString& id); virtual ~AnimInverseKinematics() override; @@ -67,14 +75,7 @@ protected: void solveTargetWithCCD(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses, bool debug); void solveTargetWithSpline(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, const glm::vec3& relTransIn, bool constrainedIn) : relRot(relRotIn), relTrans(relTransIn), constrained(constrainedIn) {} - glm::quat relRot; - glm::vec3 relTrans; - bool constrained; - }; - void debugDrawIKChain(std::map& debugJointMap, const AnimContext& context) const; + void debugDrawIKChain(JointChainInfo* jointChainInfos, size_t numJointChainInfos, const AnimContext& context) const; void debugDrawRelativePoses(const AnimContext& context) const; void debugDrawConstraints(const AnimContext& context) const; void debugDrawSpineSplines(const AnimContext& context, const std::vector& targets) const; diff --git a/libraries/animation/src/IKTarget.h b/libraries/animation/src/IKTarget.h index 011175aedf..c476e9c9da 100644 --- a/libraries/animation/src/IKTarget.h +++ b/libraries/animation/src/IKTarget.h @@ -30,10 +30,14 @@ public: const glm::vec3& getTranslation() const { return _pose.trans(); } const glm::quat& getRotation() const { return _pose.rot(); } const AnimPose& getPose() const { return _pose; } + glm::vec3 getPoleVector() const { return _poleVector; } + int getPoleIndex() const { return _poleIndex; } int getIndex() const { return _index; } Type getType() const { return _type; } void setPose(const glm::quat& rotation, const glm::vec3& translation); + void setPoleVector(const glm::vec3& poleVector) { _poleVector = poleVector; } + void setPoleIndex(int poleIndex) { _poleIndex = poleIndex; } void setIndex(int index) { _index = index; } void setType(int); void setFlexCoefficients(size_t numFlexCoefficientsIn, const float* flexCoefficientsIn); @@ -46,8 +50,10 @@ public: private: AnimPose _pose; - int _index{-1}; - Type _type{Type::RotationAndPosition}; + glm::vec3 _poleVector; + int _poleIndex { -1 }; + int _index { -1 }; + Type _type { Type::RotationAndPosition }; float _weight; float _flexCoefficients[MAX_FLEX_COEFFICIENTS]; size_t _numFlexCoefficients; From 02f06d4d4eed054943916b152bb0cef2efdb1d40 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 6 Jun 2017 17:05:06 -0700 Subject: [PATCH 02/42] WIP pole vectors work.. but still have issues... --- .../animation/src/AnimInverseKinematics.cpp | 100 +++++++++++++----- 1 file changed, 72 insertions(+), 28 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 41dae120fd..1fdeaef483 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -169,7 +169,11 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: // AJT: HACK REMOVE manually set pole vector. if (targetVar.jointName == "RightHand") { - target.setPoleVector(glm::normalize(glm::vec3(-1, 0, 0))); + target.setPoleVector(glm::normalize(glm::vec3(-1, -1, 0))); + target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); + } + if (targetVar.jointName == "LeftHand") { + target.setPoleVector(glm::normalize(glm::vec3(1, -1, 0))); target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); } @@ -478,60 +482,99 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const if (target.getPoleIndex() != -1) { - int topIndex = target.getPoleIndex(); - int midIndex = _skeleton->getParentIndex(topIndex); - if (midIndex != -1) { - int baseIndex = _skeleton->getParentIndex(midIndex); - if (baseIndex != -1) { - int baseParentIndex = _skeleton->getParentIndex(baseIndex); + int topJointIndex = target.getPoleIndex(); + int midJointIndex = _skeleton->getParentIndex(topJointIndex); + if (midJointIndex != -1) { + int baseJointIndex = _skeleton->getParentIndex(midJointIndex); + if (baseJointIndex != -1) { + int baseParentJointIndex = _skeleton->getParentIndex(baseJointIndex); AnimPose topPose, midPose, basePose, baseParentPose; + int topChainIndex = -1, baseChainIndex = -1; AnimPose postAbsPoses[MAX_CHAIN_DEPTH]; AnimPose accum = absolutePoses[_hipsIndex]; for (int i = (int)chainDepth - 1; i >= 0; i--) { accum = accum * AnimPose(glm::vec3(1.0f), jointChainInfos[i].relRot, jointChainInfos[i].relTrans); postAbsPoses[i] = accum; - if (jointChainInfos[i].jointIndex == topIndex) { + if (jointChainInfos[i].jointIndex == topJointIndex) { + topChainIndex = i; topPose = accum; } - if (jointChainInfos[i].jointIndex == midIndex) { + if (jointChainInfos[i].jointIndex == midJointIndex) { midPose = accum; } - if (jointChainInfos[i].jointIndex == baseIndex) { + if (jointChainInfos[i].jointIndex == baseJointIndex) { + baseChainIndex = i; basePose = accum; } - if (jointChainInfos[i].jointIndex == baseParentIndex) { + if (jointChainInfos[i].jointIndex == baseParentJointIndex) { baseParentPose = accum; } } - // AJT: TODO: check for parallel edge cases. - glm::vec3 d = glm::normalize(basePose.trans() - topPose.trans()); - glm::vec3 e = midPose.trans() - topPose.trans(); - glm::vec3 dHat = glm::dot(e, d) * d + topPose.trans(); - glm::vec3 eHat = glm::normalize(midPose.trans() - dHat); - float theta = acos(glm::dot(eHat, target.getPoleVector())); - glm::vec3 axis = glm::normalize(glm::cross(eHat, target.getPoleVector())); - glm::quat poleRot = glm::angleAxis(-theta, axis); + glm::quat poleRot = Quaternions::IDENTITY; + glm::vec3 d = basePose.trans() - topPose.trans(); + float dLen = glm::length(d); + if (dLen > EPSILON) { + glm::vec3 dUnit = d / dLen; + glm::vec3 e = midPose.trans() - basePose.trans(); + glm::vec3 p = target.getPoleVector(); + glm::vec3 eProj = e - glm::dot(e, dUnit) * dUnit; + glm::vec3 pProj = p - glm::dot(p, dUnit) * dUnit; + float eProjLen = glm::length(eProj); + float pProjLen = glm::length(pProj); + if (eProjLen > EPSILON && pProjLen > EPSILON) { + float dot = glm::clamp(glm::dot(eProj / eProjLen, pProj / pProjLen), 0.0f, 1.0f); + float theta = acos(dot); + glm::vec3 cross = glm::cross(eProj, pProj); + float crossLen = glm::length(cross); + if (crossLen > EPSILON) { + glm::vec3 axis = cross / crossLen; + poleRot = glm::angleAxis(theta, axis); + } + } + } if (debug) { - 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 YELLOW(1.0f, 1.0f, 0.0f, 1.0f); + const vec4 WHITE(1.0f, 1.0f, 1.0f, 1.0f); AnimPose geomToWorldPose = AnimPose(context.getRigToWorldMatrix() * context.getGeometryToRigMatrix()); + glm::vec3 dUnit = d / dLen; + glm::vec3 e = midPose.trans() - basePose.trans(); + glm::vec3 p = target.getPoleVector(); + glm::vec3 eProj = e - glm::dot(e, dUnit) * dUnit; + glm::vec3 pProj = p - glm::dot(p, dUnit) * dUnit; + float eProjLen = glm::length(eProj); + float pProjLen = glm::length(pProj); + const float POLE_VECTOR_LEN = 10.0f; - DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(basePose.trans()), geomToWorldPose.xformPoint(topPose.trans()), GREEN); - DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(dHat), geomToWorldPose.xformPoint(dHat + POLE_VECTOR_LEN * eHat), RED); - DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(dHat), geomToWorldPose.xformPoint(dHat + POLE_VECTOR_LEN * target.getPoleVector()), BLUE); + glm::vec3 midPoint = (basePose.trans() + topPose.trans()) * 0.5f; + DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(basePose.trans()), + geomToWorldPose.xformPoint(topPose.trans()), + YELLOW); + DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint), + geomToWorldPose.xformPoint(midPoint + POLE_VECTOR_LEN * glm::normalize(eProj)), + RED); + /* + DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint), + geomToWorldPose.xformPoint(midPoint + POLE_VECTOR_LEN * glm::normalize(p)), + BLUE); + */ + DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint), + geomToWorldPose.xformPoint(midPoint + POLE_VECTOR_LEN * glm::normalize(pProj)), + GREEN); } - glm::quat newBaseRelRot = glm::inverse(baseParentPose.rot()) * poleRot * basePose.rot() * glm::inverse(jointChainInfos[baseIndex].relRot); - jointChainInfos[baseIndex].relRot = glm::quat(); //newBaseRelRot * jointChainInfos[baseIndex].relRot; - glm::quat newTopRelRot = glm::inverse(midPose.rot()) * glm::inverse(poleRot) * topPose.rot() * glm::inverse(jointChainInfos[topIndex].relRot); - jointChainInfos[topIndex].relRot = glm::quat(); //newTopRelRot * jointChainInfos[topIndex].relRot; + glm::quat newBaseRelRot = glm::inverse(baseParentPose.rot()) * poleRot * basePose.rot(); + jointChainInfos[baseChainIndex].relRot = newBaseRelRot; + + glm::quat newTopRelRot = glm::inverse(midPose.rot()) * glm::inverse(poleRot) * topPose.rot(); + jointChainInfos[topChainIndex].relRot = newTopRelRot; } } } @@ -1222,7 +1265,7 @@ void AnimInverseKinematics::initConstraints() { // we determine the max/min angles by rotating the swing limit lines from parent- to child-frame // then measure the angles to swing the yAxis into alignment glm::vec3 hingeAxis = - mirror * Vectors::UNIT_Z; - const float MIN_ELBOW_ANGLE = 0.05f; + const float MIN_ELBOW_ANGLE = 0.15f; // ~8.6 deg (ajt-rad-to-deg 0.15) const float MAX_ELBOW_ANGLE = 11.0f * PI / 12.0f; glm::quat invReferenceRotation = glm::inverse(referenceRotation); glm::vec3 minSwingAxis = invReferenceRotation * glm::angleAxis(MIN_ELBOW_ANGLE, hingeAxis) * Vectors::UNIT_Y; @@ -1388,6 +1431,7 @@ void AnimInverseKinematics::debugDrawRelativePoses(const AnimContext& context) c // 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); From bb45fe03888d1e2f60b7366038e059d545f61b70 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Wed, 7 Jun 2017 10:49:27 -0700 Subject: [PATCH 03/42] WIP, straight arm still rotates.. --- libraries/animation/src/AnimInverseKinematics.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 1fdeaef483..09cee28794 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -169,11 +169,11 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: // AJT: HACK REMOVE manually set pole vector. if (targetVar.jointName == "RightHand") { - target.setPoleVector(glm::normalize(glm::vec3(-1, -1, 0))); + target.setPoleVector(glm::normalize(glm::vec3(-1, -2, -1))); target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); } if (targetVar.jointName == "LeftHand") { - target.setPoleVector(glm::normalize(glm::vec3(1, -1, 0))); + target.setPoleVector(glm::normalize(glm::vec3(1, -2, -1))); target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); } @@ -522,7 +522,8 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const glm::vec3 pProj = p - glm::dot(p, dUnit) * dUnit; float eProjLen = glm::length(eProj); float pProjLen = glm::length(pProj); - if (eProjLen > EPSILON && pProjLen > EPSILON) { + const float MIN_E_PROJ_LEN = 0.2f; // cm + if (eProjLen > MIN_E_PROJ_LEN && pProjLen > EPSILON) { float dot = glm::clamp(glm::dot(eProj / eProjLen, pProj / pProjLen), 0.0f, 1.0f); float theta = acos(dot); glm::vec3 cross = glm::cross(eProj, pProj); @@ -1265,7 +1266,7 @@ void AnimInverseKinematics::initConstraints() { // we determine the max/min angles by rotating the swing limit lines from parent- to child-frame // then measure the angles to swing the yAxis into alignment glm::vec3 hingeAxis = - mirror * Vectors::UNIT_Z; - const float MIN_ELBOW_ANGLE = 0.15f; // ~8.6 deg (ajt-rad-to-deg 0.15) + const float MIN_ELBOW_ANGLE = 0.05f; // ~2.8 deg (ajt-rad-to-deg 0.05) const float MAX_ELBOW_ANGLE = 11.0f * PI / 12.0f; glm::quat invReferenceRotation = glm::inverse(referenceRotation); glm::vec3 minSwingAxis = invReferenceRotation * glm::angleAxis(MIN_ELBOW_ANGLE, hingeAxis) * Vectors::UNIT_Y; From e8ca1a30602f050b121e7fa47d6e4619f658948d Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 9 Jun 2017 18:06:56 -0700 Subject: [PATCH 04/42] WIP: added magnitude to damping rotation near singularities. Also knee pole constraints don't work.. why? --- .../animation/src/AnimInverseKinematics.cpp | 85 ++++++++++++++----- 1 file changed, 64 insertions(+), 21 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 09cee28794..ff9a6d0d6a 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -42,6 +42,10 @@ static void lookupJointChainInfo(AnimInverseKinematics::JointChainInfo* jointCha } } +float easeOutExpo(float t) { + return 1.0f - powf(2, -10.0f * t); +} + AnimInverseKinematics::IKTargetVar::IKTargetVar(const QString& jointNameIn, const QString& positionVarIn, const QString& rotationVarIn, const QString& typeVarIn, const QString& weightVarIn, float weightIn, const std::vector& flexCoefficientsIn) : jointName(jointNameIn), @@ -166,14 +170,50 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: target.setIndex(targetVar.jointIndex); target.setWeight(weight); target.setFlexCoefficients(targetVar.numFlexCoefficients, targetVar.flexCoefficients); + target.setPoleIndex(-1); + /* // AJT: HACK REMOVE manually set pole vector. if (targetVar.jointName == "RightHand") { - target.setPoleVector(glm::normalize(glm::vec3(-1, -2, -1))); + int armJointIndex = _skeleton->nameToJointIndex("RightArm"); + glm::vec3 armPosition = _skeleton->getAbsolutePose(armJointIndex, _relativePoses).trans(); + glm::vec3 d = glm::normalize(translation - armPosition); + + // project hand y-axis onto d. + glm::vec3 handY = rotation * Vectors::UNIT_Y; + glm::vec3 handYProj = handY - glm::dot(handY, d) * d; + + // project negative pole vector on to d. + glm::vec3 p = glm::normalize(glm::vec3(-1, -3, -3)); + glm::vec3 pProj = p - glm::dot(p, d) * d; + + // compute a rotation around d that will bring the pProj to to yProj + float theta = acosf(glm::dot(glm::normalize(-pProj), glm::normalize(handYProj))); + glm::vec3 axis = glm::normalize(glm::cross(-pProj, handYProj)); + + if (glm::dot(axis, d) < 0.0f) { + // as eProjLen and pProjLen become orthognal to d, reduce the amount of rotation. + float magnitude = easeOutExpo(glm::min(glm::length(handYProj), glm::length(pProj))); + glm::quat poleAdjustRot = angleAxis(magnitude * (theta / 4.0f), axis); + target.setPoleVector(poleAdjustRot * p); + } else { + target.setPoleVector(p); + } target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); } if (targetVar.jointName == "LeftHand") { - target.setPoleVector(glm::normalize(glm::vec3(1, -2, -1))); + target.setPoleVector(glm::normalize(glm::vec3(1, -3, -3))); + target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); + } + */ + + if (targetVar.jointName == "LeftFoot") { + glm::vec3 footY = rotation * Vectors::UNIT_Y; + target.setPoleVector(footY); + target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); + } else if (targetVar.jointName == "RightFoot") { + glm::vec3 footY = rotation * Vectors::UNIT_Y; + target.setPoleVector(footY); target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); } @@ -524,13 +564,17 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const float pProjLen = glm::length(pProj); const float MIN_E_PROJ_LEN = 0.2f; // cm if (eProjLen > MIN_E_PROJ_LEN && pProjLen > EPSILON) { + + // as eProjLen and pProjLen become orthognal to d, reduce the amount of rotation. + float magnitude = easeOutExpo(glm::min(eProjLen, pProjLen)); + float dot = glm::clamp(glm::dot(eProj / eProjLen, pProj / pProjLen), 0.0f, 1.0f); - float theta = acos(dot); + float theta = acosf(dot); glm::vec3 cross = glm::cross(eProj, pProj); float crossLen = glm::length(cross); if (crossLen > EPSILON) { glm::vec3 axis = cross / crossLen; - poleRot = glm::angleAxis(theta, axis); + poleRot = glm::angleAxis(magnitude * theta, axis); } } } @@ -552,25 +596,23 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const float eProjLen = glm::length(eProj); float pProjLen = glm::length(pProj); - const float POLE_VECTOR_LEN = 10.0f; + const float PROJ_VECTOR_LEN = 10.0f; + const float POLE_VECTOR_LEN = 100.0f; glm::vec3 midPoint = (basePose.trans() + topPose.trans()) * 0.5f; DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(basePose.trans()), geomToWorldPose.xformPoint(topPose.trans()), YELLOW); DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint), - geomToWorldPose.xformPoint(midPoint + POLE_VECTOR_LEN * glm::normalize(eProj)), + geomToWorldPose.xformPoint(midPoint + PROJ_VECTOR_LEN * glm::normalize(eProj)), RED); - /* + DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint), + geomToWorldPose.xformPoint(midPoint + PROJ_VECTOR_LEN * glm::normalize(pProj)), + GREEN); DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint), geomToWorldPose.xformPoint(midPoint + POLE_VECTOR_LEN * glm::normalize(p)), BLUE); - */ - DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint), - geomToWorldPose.xformPoint(midPoint + POLE_VECTOR_LEN * glm::normalize(pProj)), - GREEN); } - glm::quat newBaseRelRot = glm::inverse(baseParentPose.rot()) * poleRot * basePose.rot(); jointChainInfos[baseChainIndex].relRot = newBaseRelRot; @@ -672,7 +714,8 @@ const std::vector* AnimInverseKinematics void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses, bool debug) { - std::map debugJointMap; + const size_t MAX_CHAIN_DEPTH = 30; + JointChainInfo jointChainInfos[MAX_CHAIN_DEPTH]; const int baseIndex = _hipsIndex; @@ -732,7 +775,6 @@ void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, co ::blend(1, &absolutePoses[splineJointInfo.jointIndex], &desiredAbsPose, target.getFlexCoefficient(i), &flexedAbsPose); AnimPose relPose = parentAbsPose.inverse() * flexedAbsPose; - _rotationAccumulators[splineJointInfo.jointIndex].add(relPose.rot(), target.getWeight()); bool constrained = false; if (splineJointInfo.jointIndex != _hipsIndex) { @@ -756,18 +798,19 @@ void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, co } } - _translationAccumulators[splineJointInfo.jointIndex].add(relPose.trans(), target.getWeight()); - - if (debug) { - debugJointMap[splineJointInfo.jointIndex] = DebugJoint(relPose.rot(), relPose.trans(), constrained); - } + jointChainInfos[i] = { relPose.rot(), relPose.trans(), target.getWeight(), splineJointInfo.jointIndex, constrained }; parentAbsPose = flexedAbsPose; } } + for (size_t i = 0; i < splineJointInfoVec->size(); i++) { + _rotationAccumulators[jointChainInfos[i].jointIndex].add(jointChainInfos[i].relRot, jointChainInfos[i].weight); + _translationAccumulators[jointChainInfos[i].jointIndex].add(jointChainInfos[i].relTrans, jointChainInfos[i].weight); + } + if (debug) { - debugDrawIKChain(debugJointMap, context); + debugDrawIKChain(jointChainInfos, splineJointInfoVec->size(), context); } } @@ -1611,7 +1654,7 @@ void AnimInverseKinematics::debugDrawConstraints(const AnimContext& context) con glm::vec3 worldSwungAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * refRot * swungAxis); glm::vec3 swingTip = pos + SWING_LENGTH * worldSwungAxis; - float prevPhi = acos(swingTwistConstraint->getMinDots()[j]); + float prevPhi = acosf(swingTwistConstraint->getMinDots()[j]); float prevTheta = theta - D_THETA; glm::vec3 prevSwungAxis = sphericalToCartesian(prevPhi, prevTheta - PI_2); glm::vec3 prevWorldSwungAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * refRot * prevSwungAxis); From 0cde22d937175822583236e4f71787eba50d6fa5 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 12 Jun 2017 12:13:03 -0700 Subject: [PATCH 05/42] Bug fix for pole constraint on legs --- libraries/animation/src/AnimInverseKinematics.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index ff9a6d0d6a..7de6626cd5 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -528,10 +528,11 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const int baseJointIndex = _skeleton->getParentIndex(midJointIndex); if (baseJointIndex != -1) { int baseParentJointIndex = _skeleton->getParentIndex(baseJointIndex); - AnimPose topPose, midPose, basePose, baseParentPose; + AnimPose topPose, midPose, basePose; int topChainIndex = -1, baseChainIndex = -1; AnimPose postAbsPoses[MAX_CHAIN_DEPTH]; AnimPose accum = absolutePoses[_hipsIndex]; + AnimPose baseParentPose = absolutePoses[_hipsIndex]; for (int i = (int)chainDepth - 1; i >= 0; i--) { accum = accum * AnimPose(glm::vec3(1.0f), jointChainInfos[i].relRot, jointChainInfos[i].relTrans); postAbsPoses[i] = accum; @@ -1341,7 +1342,7 @@ void AnimInverseKinematics::initConstraints() { // we determine the max/min angles by rotating the swing limit lines from parent- to child-frame // then measure the angles to swing the yAxis into alignment const float MIN_KNEE_ANGLE = 0.097f; // ~5 deg - const float MAX_KNEE_ANGLE = 7.0f * PI / 8.0f; + const float MAX_KNEE_ANGLE = 7.0f * PI / 8.0f; // 157.5 deg glm::quat invReferenceRotation = glm::inverse(referenceRotation); glm::vec3 minSwingAxis = invReferenceRotation * glm::angleAxis(MIN_KNEE_ANGLE, hingeAxis) * Vectors::UNIT_Y; glm::vec3 maxSwingAxis = invReferenceRotation * glm::angleAxis(MAX_KNEE_ANGLE, hingeAxis) * Vectors::UNIT_Y; From 7521d6124e3502ebf4097489d304eccabfccd7bd Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 12 Jun 2017 17:23:23 -0700 Subject: [PATCH 06/42] WIP: added blend between hips and foot for knee pole vector --- .../animation/src/AnimInverseKinematics.cpp | 31 +++++++++++++------ 1 file changed, 21 insertions(+), 10 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 7de6626cd5..0ff236d1b1 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -161,9 +161,9 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: IKTarget target; target.setType(animVars.lookup(targetVar.typeVar, (int)IKTarget::Type::RotationAndPosition)); if (target.getType() != IKTarget::Type::Unknown) { - AnimPose defaultPose = _skeleton->getAbsolutePose(targetVar.jointIndex, underPoses); - glm::quat rotation = animVars.lookupRigToGeometry(targetVar.rotationVar, defaultPose.rot()); - glm::vec3 translation = animVars.lookupRigToGeometry(targetVar.positionVar, defaultPose.trans()); + AnimPose absPose = _skeleton->getAbsolutePose(targetVar.jointIndex, underPoses); + glm::quat rotation = animVars.lookupRigToGeometry(targetVar.rotationVar, absPose.rot()); + glm::vec3 translation = animVars.lookupRigToGeometry(targetVar.positionVar, absPose.trans()); float weight = animVars.lookup(targetVar.weightVar, targetVar.weight); target.setPose(rotation, translation); @@ -207,13 +207,24 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: } */ - if (targetVar.jointName == "LeftFoot") { - glm::vec3 footY = rotation * Vectors::UNIT_Y; - target.setPoleVector(footY); - target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); - } else if (targetVar.jointName == "RightFoot") { - glm::vec3 footY = rotation * Vectors::UNIT_Y; - target.setPoleVector(footY); + if (targetVar.jointName == "LeftFoot" || targetVar.jointName == "RightFoot") { + + // compute the forward direction of the foot. + AnimPose defaultPose = _skeleton->getAbsoluteDefaultPose(targetVar.jointIndex); + glm::vec3 localForward = glm::inverse(defaultPose.rot()) * Vectors::UNIT_Z; + glm::vec3 footForward = rotation * localForward; + + // compute the forward direction of the hips. + glm::quat hipsRotation = _skeleton->getAbsolutePose(_hipsIndex, _relativePoses).rot(); + glm::vec3 hipsForward = hipsRotation * Vectors::UNIT_Z; + + // blend between the hips and the foot. + const float FOOT_TO_HIPS_BLEND_FACTOR = 0.5f; + glm::vec3 poleVector = glm::normalize(lerp(footForward, hipsForward, FOOT_TO_HIPS_BLEND_FACTOR)); + + // TODO: smooth toward desired pole vector from previous pole vector... to reduce jitter + + target.setPoleVector(poleVector); target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); } From f20c03fa6e033bad78278c827137ceba05f7d72d Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 13 Jun 2017 15:36:05 -0700 Subject: [PATCH 07/42] Pole vectors can be controlled via anim vars. --- .../resources/avatar/avatar-animation.json | 8 +- .../animation/src/AnimInverseKinematics.cpp | 73 ++++------------- .../animation/src/AnimInverseKinematics.h | 8 +- libraries/animation/src/AnimNodeLoader.cpp | 4 +- libraries/animation/src/Rig.cpp | 80 ++++++++++++++----- libraries/animation/src/Rig.h | 12 ++- libraries/render-utils/src/Model.cpp | 4 - libraries/render-utils/src/Model.h | 1 - 8 files changed, 96 insertions(+), 94 deletions(-) diff --git a/interface/resources/avatar/avatar-animation.json b/interface/resources/avatar/avatar-animation.json index 1412b45968..914fe8ba69 100644 --- a/interface/resources/avatar/avatar-animation.json +++ b/interface/resources/avatar/avatar-animation.json @@ -86,7 +86,9 @@ "typeVar": "rightFootType", "weightVar": "rightFootWeight", "weight": 1.0, - "flexCoefficients": [1, 0.45, 0.45] + "flexCoefficients": [1, 0.45, 0.45], + "poleJointNameVar": "rightFootPoleJointName", + "poleVectorVar": "rightFootPoleVector" }, { "jointName": "LeftFoot", @@ -95,7 +97,9 @@ "typeVar": "leftFootType", "weightVar": "leftFootWeight", "weight": 1.0, - "flexCoefficients": [1, 0.45, 0.45] + "flexCoefficients": [1, 0.45, 0.45], + "poleJointNameVar": "leftFootPoleJointName", + "poleVectorVar": "leftFootPoleVector" }, { "jointName": "Spine2", diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 0ff236d1b1..57fa6fd1e3 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -47,12 +47,15 @@ float easeOutExpo(float t) { } AnimInverseKinematics::IKTargetVar::IKTargetVar(const QString& jointNameIn, const QString& positionVarIn, const QString& rotationVarIn, - const QString& typeVarIn, const QString& weightVarIn, float weightIn, const std::vector& flexCoefficientsIn) : + const QString& typeVarIn, const QString& weightVarIn, float weightIn, const std::vector& flexCoefficientsIn, + const QString& poleJointNameVarIn, const QString& poleVectorVarIn) : jointName(jointNameIn), positionVar(positionVarIn), rotationVar(rotationVarIn), typeVar(typeVarIn), weightVar(weightVarIn), + poleJointNameVar(poleJointNameVarIn), + poleVectorVar(poleVectorVarIn), weight(weightIn), numFlexCoefficients(flexCoefficientsIn.size()), jointIndex(-1) @@ -69,6 +72,8 @@ AnimInverseKinematics::IKTargetVar::IKTargetVar(const IKTargetVar& orig) : rotationVar(orig.rotationVar), typeVar(orig.typeVar), weightVar(orig.weightVar), + poleJointNameVar(orig.poleJointNameVar), + poleVectorVar(orig.poleVectorVar), weight(orig.weight), numFlexCoefficients(orig.numFlexCoefficients), jointIndex(orig.jointIndex) @@ -122,8 +127,9 @@ void AnimInverseKinematics::computeAbsolutePoses(AnimPoseVec& absolutePoses) con } void AnimInverseKinematics::setTargetVars(const QString& jointName, const QString& positionVar, const QString& rotationVar, - const QString& typeVar, const QString& weightVar, float weight, const std::vector& flexCoefficients) { - IKTargetVar targetVar(jointName, positionVar, rotationVar, typeVar, weightVar, weight, flexCoefficients); + const QString& typeVar, const QString& weightVar, float weight, const std::vector& flexCoefficients, + const QString& poleJointNameVar, const QString& poleVectorVar) { + IKTargetVar targetVar(jointName, positionVar, rotationVar, typeVar, weightVar, weight, flexCoefficients, poleJointNameVar, poleVectorVar); // if there are dups, last one wins. bool found = false; @@ -170,63 +176,14 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: target.setIndex(targetVar.jointIndex); target.setWeight(weight); target.setFlexCoefficients(targetVar.numFlexCoefficients, targetVar.flexCoefficients); - target.setPoleIndex(-1); - /* - // AJT: HACK REMOVE manually set pole vector. - if (targetVar.jointName == "RightHand") { - int armJointIndex = _skeleton->nameToJointIndex("RightArm"); - glm::vec3 armPosition = _skeleton->getAbsolutePose(armJointIndex, _relativePoses).trans(); - glm::vec3 d = glm::normalize(translation - armPosition); + // AJT: TODO: cache the pole joint index. + QString poleJointName = animVars.lookup(targetVar.poleJointNameVar, QString("")); + int poleJointIndex = _skeleton->nameToJointIndex(poleJointName); + target.setPoleIndex(poleJointIndex); - // project hand y-axis onto d. - glm::vec3 handY = rotation * Vectors::UNIT_Y; - glm::vec3 handYProj = handY - glm::dot(handY, d) * d; - - // project negative pole vector on to d. - glm::vec3 p = glm::normalize(glm::vec3(-1, -3, -3)); - glm::vec3 pProj = p - glm::dot(p, d) * d; - - // compute a rotation around d that will bring the pProj to to yProj - float theta = acosf(glm::dot(glm::normalize(-pProj), glm::normalize(handYProj))); - glm::vec3 axis = glm::normalize(glm::cross(-pProj, handYProj)); - - if (glm::dot(axis, d) < 0.0f) { - // as eProjLen and pProjLen become orthognal to d, reduce the amount of rotation. - float magnitude = easeOutExpo(glm::min(glm::length(handYProj), glm::length(pProj))); - glm::quat poleAdjustRot = angleAxis(magnitude * (theta / 4.0f), axis); - target.setPoleVector(poleAdjustRot * p); - } else { - target.setPoleVector(p); - } - target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); - } - if (targetVar.jointName == "LeftHand") { - target.setPoleVector(glm::normalize(glm::vec3(1, -3, -3))); - target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); - } - */ - - if (targetVar.jointName == "LeftFoot" || targetVar.jointName == "RightFoot") { - - // compute the forward direction of the foot. - AnimPose defaultPose = _skeleton->getAbsoluteDefaultPose(targetVar.jointIndex); - glm::vec3 localForward = glm::inverse(defaultPose.rot()) * Vectors::UNIT_Z; - glm::vec3 footForward = rotation * localForward; - - // compute the forward direction of the hips. - glm::quat hipsRotation = _skeleton->getAbsolutePose(_hipsIndex, _relativePoses).rot(); - glm::vec3 hipsForward = hipsRotation * Vectors::UNIT_Z; - - // blend between the hips and the foot. - const float FOOT_TO_HIPS_BLEND_FACTOR = 0.5f; - glm::vec3 poleVector = glm::normalize(lerp(footForward, hipsForward, FOOT_TO_HIPS_BLEND_FACTOR)); - - // TODO: smooth toward desired pole vector from previous pole vector... to reduce jitter - - target.setPoleVector(poleVector); - target.setPoleIndex(_skeleton->nameToJointIndex(targetVar.jointName)); - } + glm::vec3 poleVector = animVars.lookupRigToGeometryVector(targetVar.poleVectorVar, Vectors::UNIT_Z); + target.setPoleVector(glm::normalize(poleVector)); targets.push_back(target); diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index 2ed96ed6ed..946c23883e 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -42,7 +42,8 @@ public: void computeAbsolutePoses(AnimPoseVec& absolutePoses) const; void setTargetVars(const QString& jointName, const QString& positionVar, const QString& rotationVar, - const QString& typeVar, const QString& weightVar, float weight, const std::vector& flexCoefficients); + const QString& typeVar, const QString& weightVar, float weight, const std::vector& flexCoefficients, + const QString& poleJointNameVar, const QString& poleVectorVar); virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimNode::Triggers& triggersOut) override; virtual const AnimPoseVec& overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) override; @@ -108,7 +109,8 @@ protected: enum FlexCoefficients { MAX_FLEX_COEFFICIENTS = 10 }; struct IKTargetVar { IKTargetVar(const QString& jointNameIn, const QString& positionVarIn, const QString& rotationVarIn, - const QString& typeVarIn, const QString& weightVarIn, float weightIn, const std::vector& flexCoefficientsIn); + const QString& typeVarIn, const QString& weightVarIn, float weightIn, const std::vector& flexCoefficientsIn, + const QString& poleJointNameVar, const QString& poleVectorVar); IKTargetVar(const IKTargetVar& orig); QString jointName; @@ -116,6 +118,8 @@ protected: QString rotationVar; QString typeVar; QString weightVar; + QString poleJointNameVar; + QString poleVectorVar; float weight; float flexCoefficients[MAX_FLEX_COEFFICIENTS]; size_t numFlexCoefficients; diff --git a/libraries/animation/src/AnimNodeLoader.cpp b/libraries/animation/src/AnimNodeLoader.cpp index 44ed8c6053..c4a149a35b 100644 --- a/libraries/animation/src/AnimNodeLoader.cpp +++ b/libraries/animation/src/AnimNodeLoader.cpp @@ -479,6 +479,8 @@ AnimNode::Pointer loadInverseKinematicsNode(const QJsonObject& jsonObj, const QS READ_OPTIONAL_STRING(typeVar, targetObj); READ_OPTIONAL_STRING(weightVar, targetObj); READ_OPTIONAL_FLOAT(weight, targetObj, 1.0f); + READ_OPTIONAL_STRING(poleJointNameVar, targetObj); + READ_OPTIONAL_STRING(poleVectorVar, targetObj); auto flexCoefficientsValue = targetObj.value("flexCoefficients"); if (!flexCoefficientsValue.isArray()) { @@ -491,7 +493,7 @@ AnimNode::Pointer loadInverseKinematicsNode(const QJsonObject& jsonObj, const QS flexCoefficients.push_back((float)value.toDouble()); } - node->setTargetVars(jointName, positionVar, rotationVar, typeVar, weightVar, weight, flexCoefficients); + node->setTargetVars(jointName, positionVar, rotationVar, typeVar, weightVar, weight, flexCoefficients, poleJointNameVar, poleVectorVar); }; READ_OPTIONAL_STRING(solutionSource, jsonObj); diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 20a2aab2b6..2966e882d2 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -479,12 +479,6 @@ bool Rig::getAbsoluteJointPoseInRigFrame(int jointIndex, AnimPose& returnPose) c } } -bool Rig::getJointCombinedRotation(int jointIndex, glm::quat& result, const glm::quat& rotation) const { - // AJT: TODO: used by attachments - ASSERT(false); - return false; -} - void Rig::calcAnimAlpha(float speed, const std::vector& referenceSpeeds, float* alphaOut) const { ASSERT(referenceSpeeds.size() > 0); @@ -950,6 +944,7 @@ void Rig::updateAnimations(float deltaTime, const glm::mat4& rootTransform, cons // evaluate the animation AnimNode::Triggers triggersOut; + _internalPoseSet._relativePoses = _animNode->evaluate(_animVars, context, deltaTime, triggersOut); if ((int)_internalPoseSet._relativePoses.size() != _animSkeleton->getNumJoints()) { // animations haven't fully loaded yet. @@ -1145,6 +1140,21 @@ void Rig::updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm } } +glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, const glm::quat& footTargetOrientation, int hipsIndex) const { + + AnimPose defaultPose = _animSkeleton->getAbsoluteDefaultPose(footJointIndex); + glm::vec3 localForward = glm::inverse(defaultPose.rot()) * Vectors::UNIT_Z; + glm::vec3 footForward = footTargetOrientation * localForward; + + // compute the forward direction of the hips. + glm::quat hipsRotation = _externalPoseSet._absolutePoses[hipsIndex].rot(); + glm::vec3 hipsForward = hipsRotation * Vectors::UNIT_Z; + + // blend between the hips and the foot. + const float FOOT_TO_HIPS_BLEND_FACTOR = 0.5f; + return glm::normalize(lerp(footForward, hipsForward, FOOT_TO_HIPS_BLEND_FACTOR)); +} + void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, float dt) { if (_animSkeleton && _animNode) { const float HAND_RADIUS = 0.05f; @@ -1255,16 +1265,46 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f _animVars.set("leftFootPosition", params.leftFootPosition); _animVars.set("leftFootRotation", params.leftFootOrientation); _animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition); + + int footJointIndex = _animSkeleton->nameToJointIndex("LeftFoot"); + glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, params.leftFootOrientation, hipsIndex); + + // smooth toward desired pole vector from previous pole vector... to reduce jitter + if (!_prevLeftFootPoleVectorValid) { + _prevLeftFootPoleVectorValid = true; + _prevLeftFootPoleVector = poleVector; + } + const float POLE_VECTOR_BLEND_FACTOR = 0.9f; + _prevLeftFootPoleVector = lerp(poleVector, _prevLeftFootPoleVector, POLE_VECTOR_BLEND_FACTOR); + + _animVars.set("leftFootPoleVector", _prevLeftFootPoleVector); + _animVars.set("leftFootPoleJointName", QString("LeftFoot")); + } else { _animVars.unset("leftFootPosition"); _animVars.unset("leftFootRotation"); _animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition); + _prevLeftFootPoleVectorValid = false; } if (params.isRightFootEnabled) { _animVars.set("rightFootPosition", params.rightFootPosition); _animVars.set("rightFootRotation", params.rightFootOrientation); _animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition); + + int footJointIndex = _animSkeleton->nameToJointIndex("RightFoot"); + glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, params.rightFootOrientation, hipsIndex); + + // smooth toward desired pole vector from previous pole vector... to reduce jitter + if (!_prevRightFootPoleVectorValid) { + _prevRightFootPoleVectorValid = true; + _prevRightFootPoleVector = poleVector; + } + const float POLE_VECTOR_BLEND_FACTOR = 0.9f; + _prevRightFootPoleVector = lerp(poleVector, _prevRightFootPoleVector, POLE_VECTOR_BLEND_FACTOR); + + _animVars.set("rightFootPoleVector", _prevRightFootPoleVector); + _animVars.set("rightFootPoleJointName", QString("RightFoot")); } else { _animVars.unset("rightFootPosition"); _animVars.unset("rightFootRotation"); @@ -1455,22 +1495,18 @@ void Rig::computeAvatarBoundingCapsule( AnimInverseKinematics ikNode("boundingShape"); ikNode.setSkeleton(_animSkeleton); - ikNode.setTargetVars("LeftHand", - "leftHandPosition", - "leftHandRotation", - "leftHandType", "leftHandWeight", 1.0f, {}); - ikNode.setTargetVars("RightHand", - "rightHandPosition", - "rightHandRotation", - "rightHandType", "rightHandWeight", 1.0f, {}); - ikNode.setTargetVars("LeftFoot", - "leftFootPosition", - "leftFootRotation", - "leftFootType", "leftFootWeight", 1.0f, {}); - ikNode.setTargetVars("RightFoot", - "rightFootPosition", - "rightFootRotation", - "rightFootType", "rightFootWeight", 1.0f, {}); + ikNode.setTargetVars("LeftHand", "leftHandPosition", "leftHandRotation", + "leftHandType", "leftHandWeight", 1.0f, {}, + "leftHandPoleJointName", "leftHandPoleVector"); + ikNode.setTargetVars("RightHand", "rightHandPosition", "rightHandRotation", + "rightHandType", "rightHandWeight", 1.0f, {}, + "rightHandPoleJointName", "rightHandPoleVector"); + ikNode.setTargetVars("LeftFoot", "leftFootPosition", "leftFootRotation", + "leftFootType", "leftFootWeight", 1.0f, {}, + "leftFootPoleJointName", "leftFootPoleVector"); + ikNode.setTargetVars("RightFoot", "rightFootPosition", "rightFootRotation", + "rightFootType", "rightFootWeight", 1.0f, {}, + "rightFootPoleJointName", "rightFootPoleVector"); AnimPose geometryToRig = _modelOffset * _geometryOffset; diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 994bd4b074..4fc5f76146 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -153,9 +153,6 @@ public: bool getAbsoluteJointTranslationInRigFrame(int jointIndex, glm::vec3& translation) const; bool getAbsoluteJointPoseInRigFrame(int jointIndex, AnimPose& returnPose) const; - // legacy - bool getJointCombinedRotation(int jointIndex, glm::quat& result, const glm::quat& rotation) const; - // rig space glm::mat4 getJointTransform(int jointIndex) const; @@ -252,6 +249,8 @@ protected: void updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm::quat& modelRotation, const glm::vec3& lookAt, const glm::vec3& saccade); void calcAnimAlpha(float speed, const std::vector& referenceSpeeds, float* alphaOut) const; + glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, const glm::quat& footTargetOrientation, int hipsIndex) const; + AnimPose _modelOffset; // model to rig space AnimPose _geometryOffset; // geometry to model space (includes unit offset & fst offsets) AnimPose _invGeometryOffset; @@ -347,7 +346,6 @@ protected: bool _enableDebugDrawIKConstraints { false }; bool _enableDebugDrawIKChains { false }; -private: QMap _stateHandlers; int _nextStateHandlerId { 0 }; QMutex _stateMutex; @@ -358,6 +356,12 @@ private: float _rightHandRelaxDuration { 0.0f }; AnimPose _lastLeftHandControlledPose; AnimPose _lastRightHandControlledPose; + + glm::vec3 _prevRightFootPoleVector = { Vectors::UNIT_Z }; + bool _prevRightFootPoleVectorValid = { false }; + + glm::vec3 _prevLeftFootPoleVector = { Vectors::UNIT_Z }; + bool _prevLeftFootPoleVectorValid = { false }; }; #endif /* defined(__hifi__Rig__) */ diff --git a/libraries/render-utils/src/Model.cpp b/libraries/render-utils/src/Model.cpp index 447d0e37bd..8953ea94f0 100644 --- a/libraries/render-utils/src/Model.cpp +++ b/libraries/render-utils/src/Model.cpp @@ -866,10 +866,6 @@ bool Model::getRelativeDefaultJointTranslation(int jointIndex, glm::vec3& transl return _rig.getRelativeDefaultJointTranslation(jointIndex, translationOut); } -bool Model::getJointCombinedRotation(int jointIndex, glm::quat& rotation) const { - return _rig.getJointCombinedRotation(jointIndex, rotation, _rotation); -} - QStringList Model::getJointNames() const { if (QThread::currentThread() != thread()) { QStringList result; diff --git a/libraries/render-utils/src/Model.h b/libraries/render-utils/src/Model.h index 53d446d306..a0da8a7a8c 100644 --- a/libraries/render-utils/src/Model.h +++ b/libraries/render-utils/src/Model.h @@ -176,7 +176,6 @@ public: int getJointStateCount() const { return (int)_rig.getJointStateCount(); } bool getJointPositionInWorldFrame(int jointIndex, glm::vec3& position) const; bool getJointRotationInWorldFrame(int jointIndex, glm::quat& rotation) const; - bool getJointCombinedRotation(int jointIndex, glm::quat& rotation) const; /// \param jointIndex index of joint in model structure /// \param rotation[out] rotation of joint in model-frame From 10f94c2d605e8d7eddf38b57e82ba68afe30022f Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Wed, 14 Jun 2017 11:35:53 -0700 Subject: [PATCH 08/42] Fixed pole vectors on straight limbs by using reference vector --- .../resources/avatar/avatar-animation.json | 6 ++- .../animation/src/AnimInverseKinematics.cpp | 37 ++++++++++--------- .../animation/src/AnimInverseKinematics.h | 7 ++-- libraries/animation/src/AnimNodeLoader.cpp | 5 ++- libraries/animation/src/IKTarget.h | 9 +++-- libraries/animation/src/Rig.cpp | 21 ++++++----- 6 files changed, 48 insertions(+), 37 deletions(-) diff --git a/interface/resources/avatar/avatar-animation.json b/interface/resources/avatar/avatar-animation.json index 914fe8ba69..c9552a3ead 100644 --- a/interface/resources/avatar/avatar-animation.json +++ b/interface/resources/avatar/avatar-animation.json @@ -87,7 +87,8 @@ "weightVar": "rightFootWeight", "weight": 1.0, "flexCoefficients": [1, 0.45, 0.45], - "poleJointNameVar": "rightFootPoleJointName", + "poleVectorEnabledVar": "rightFootPoleVectorEnabled", + "poleReferenceVectorVar": "rightFootPoleReferenceVector", "poleVectorVar": "rightFootPoleVector" }, { @@ -98,7 +99,8 @@ "weightVar": "leftFootWeight", "weight": 1.0, "flexCoefficients": [1, 0.45, 0.45], - "poleJointNameVar": "leftFootPoleJointName", + "poleVectorEnabledVar": "leftFootPoleVectorEnabled", + "poleReferenceVectorVar": "leftFootPoleReferenceVector", "poleVectorVar": "leftFootPoleVector" }, { diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 57fa6fd1e3..2a493a96ab 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -48,13 +48,14 @@ float easeOutExpo(float t) { AnimInverseKinematics::IKTargetVar::IKTargetVar(const QString& jointNameIn, const QString& positionVarIn, const QString& rotationVarIn, const QString& typeVarIn, const QString& weightVarIn, float weightIn, const std::vector& flexCoefficientsIn, - const QString& poleJointNameVarIn, const QString& poleVectorVarIn) : + const QString& poleVectorEnabledVarIn, const QString& poleReferenceVectorVarIn, const QString& poleVectorVarIn) : jointName(jointNameIn), positionVar(positionVarIn), rotationVar(rotationVarIn), typeVar(typeVarIn), weightVar(weightVarIn), - poleJointNameVar(poleJointNameVarIn), + poleVectorEnabledVar(poleVectorEnabledVarIn), + poleReferenceVectorVar(poleReferenceVectorVarIn), poleVectorVar(poleVectorVarIn), weight(weightIn), numFlexCoefficients(flexCoefficientsIn.size()), @@ -72,7 +73,8 @@ AnimInverseKinematics::IKTargetVar::IKTargetVar(const IKTargetVar& orig) : rotationVar(orig.rotationVar), typeVar(orig.typeVar), weightVar(orig.weightVar), - poleJointNameVar(orig.poleJointNameVar), + poleVectorEnabledVar(orig.poleVectorEnabledVar), + poleReferenceVectorVar(orig.poleReferenceVectorVar), poleVectorVar(orig.poleVectorVar), weight(orig.weight), numFlexCoefficients(orig.numFlexCoefficients), @@ -128,8 +130,8 @@ void AnimInverseKinematics::computeAbsolutePoses(AnimPoseVec& absolutePoses) con void AnimInverseKinematics::setTargetVars(const QString& jointName, const QString& positionVar, const QString& rotationVar, const QString& typeVar, const QString& weightVar, float weight, const std::vector& flexCoefficients, - const QString& poleJointNameVar, const QString& poleVectorVar) { - IKTargetVar targetVar(jointName, positionVar, rotationVar, typeVar, weightVar, weight, flexCoefficients, poleJointNameVar, poleVectorVar); + const QString& poleVectorEnabledVar, const QString& poleReferenceVectorVar, const QString& poleVectorVar) { + IKTargetVar targetVar(jointName, positionVar, rotationVar, typeVar, weightVar, weight, flexCoefficients, poleVectorEnabledVar, poleReferenceVectorVar, poleVectorVar); // if there are dups, last one wins. bool found = false; @@ -177,14 +179,15 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: target.setWeight(weight); target.setFlexCoefficients(targetVar.numFlexCoefficients, targetVar.flexCoefficients); - // AJT: TODO: cache the pole joint index. - QString poleJointName = animVars.lookup(targetVar.poleJointNameVar, QString("")); - int poleJointIndex = _skeleton->nameToJointIndex(poleJointName); - target.setPoleIndex(poleJointIndex); + bool poleVectorEnabled = animVars.lookup(targetVar.poleVectorEnabledVar, false); + target.setPoleVectorEnabled(poleVectorEnabled); glm::vec3 poleVector = animVars.lookupRigToGeometryVector(targetVar.poleVectorVar, Vectors::UNIT_Z); target.setPoleVector(glm::normalize(poleVector)); + glm::vec3 poleReferenceVector = animVars.lookupRigToGeometryVector(targetVar.poleReferenceVectorVar, Vectors::UNIT_Z); + target.setPoleReferenceVector(glm::normalize(poleReferenceVector)); + targets.push_back(target); if (targetVar.jointIndex > _maxTargetIndex) { @@ -488,9 +491,8 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const chainDepth++; } - if (target.getPoleIndex() != -1) { - - int topJointIndex = target.getPoleIndex(); + if (target.getPoleVectorEnabled()) { + int topJointIndex = target.getIndex(); int midJointIndex = _skeleton->getParentIndex(topJointIndex); if (midJointIndex != -1) { int baseJointIndex = _skeleton->getParentIndex(midJointIndex); @@ -525,17 +527,16 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const float dLen = glm::length(d); if (dLen > EPSILON) { glm::vec3 dUnit = d / dLen; - glm::vec3 e = midPose.trans() - basePose.trans(); + glm::vec3 e = midPose.xformVector(target.getPoleReferenceVector()); glm::vec3 p = target.getPoleVector(); glm::vec3 eProj = e - glm::dot(e, dUnit) * dUnit; glm::vec3 pProj = p - glm::dot(p, dUnit) * dUnit; float eProjLen = glm::length(eProj); float pProjLen = glm::length(pProj); - const float MIN_E_PROJ_LEN = 0.2f; // cm - if (eProjLen > MIN_E_PROJ_LEN && pProjLen > EPSILON) { + if (eProjLen > EPSILON && pProjLen > EPSILON) { - // as eProjLen and pProjLen become orthognal to d, reduce the amount of rotation. - float magnitude = easeOutExpo(glm::min(eProjLen, pProjLen)); + // as pProjLen become orthognal to d, reduce the amount of rotation. + float magnitude = easeOutExpo(pProjLen); float dot = glm::clamp(glm::dot(eProj / eProjLen, pProj / pProjLen), 0.0f, 1.0f); float theta = acosf(dot); @@ -558,7 +559,7 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const AnimPose geomToWorldPose = AnimPose(context.getRigToWorldMatrix() * context.getGeometryToRigMatrix()); glm::vec3 dUnit = d / dLen; - glm::vec3 e = midPose.trans() - basePose.trans(); + glm::vec3 e = midPose.xformVector(target.getPoleReferenceVector()); glm::vec3 p = target.getPoleVector(); glm::vec3 eProj = e - glm::dot(e, dUnit) * dUnit; glm::vec3 pProj = p - glm::dot(p, dUnit) * dUnit; diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index 946c23883e..016317aee3 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -43,7 +43,7 @@ public: void setTargetVars(const QString& jointName, const QString& positionVar, const QString& rotationVar, const QString& typeVar, const QString& weightVar, float weight, const std::vector& flexCoefficients, - const QString& poleJointNameVar, const QString& poleVectorVar); + const QString& poleVectorEnabledVar, const QString& poleReferenceVectorVar, const QString& poleVectorVar); virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimNode::Triggers& triggersOut) override; virtual const AnimPoseVec& overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) override; @@ -110,7 +110,7 @@ protected: struct IKTargetVar { IKTargetVar(const QString& jointNameIn, const QString& positionVarIn, const QString& rotationVarIn, const QString& typeVarIn, const QString& weightVarIn, float weightIn, const std::vector& flexCoefficientsIn, - const QString& poleJointNameVar, const QString& poleVectorVar); + const QString& poleVectorEnabledVar, const QString& poleReferenceVectorVar, const QString& poleVectorVar); IKTargetVar(const IKTargetVar& orig); QString jointName; @@ -118,7 +118,8 @@ protected: QString rotationVar; QString typeVar; QString weightVar; - QString poleJointNameVar; + QString poleVectorEnabledVar; + QString poleReferenceVectorVar; QString poleVectorVar; float weight; float flexCoefficients[MAX_FLEX_COEFFICIENTS]; diff --git a/libraries/animation/src/AnimNodeLoader.cpp b/libraries/animation/src/AnimNodeLoader.cpp index c4a149a35b..2a1978127d 100644 --- a/libraries/animation/src/AnimNodeLoader.cpp +++ b/libraries/animation/src/AnimNodeLoader.cpp @@ -479,7 +479,8 @@ AnimNode::Pointer loadInverseKinematicsNode(const QJsonObject& jsonObj, const QS READ_OPTIONAL_STRING(typeVar, targetObj); READ_OPTIONAL_STRING(weightVar, targetObj); READ_OPTIONAL_FLOAT(weight, targetObj, 1.0f); - READ_OPTIONAL_STRING(poleJointNameVar, targetObj); + READ_OPTIONAL_STRING(poleVectorEnabledVar, targetObj); + READ_OPTIONAL_STRING(poleReferenceVectorVar, targetObj); READ_OPTIONAL_STRING(poleVectorVar, targetObj); auto flexCoefficientsValue = targetObj.value("flexCoefficients"); @@ -493,7 +494,7 @@ AnimNode::Pointer loadInverseKinematicsNode(const QJsonObject& jsonObj, const QS flexCoefficients.push_back((float)value.toDouble()); } - node->setTargetVars(jointName, positionVar, rotationVar, typeVar, weightVar, weight, flexCoefficients, poleJointNameVar, poleVectorVar); + node->setTargetVars(jointName, positionVar, rotationVar, typeVar, weightVar, weight, flexCoefficients, poleVectorEnabledVar, poleReferenceVectorVar, poleVectorVar); }; READ_OPTIONAL_STRING(solutionSource, jsonObj); diff --git a/libraries/animation/src/IKTarget.h b/libraries/animation/src/IKTarget.h index c476e9c9da..5567539659 100644 --- a/libraries/animation/src/IKTarget.h +++ b/libraries/animation/src/IKTarget.h @@ -31,13 +31,15 @@ public: const glm::quat& getRotation() const { return _pose.rot(); } const AnimPose& getPose() const { return _pose; } glm::vec3 getPoleVector() const { return _poleVector; } - int getPoleIndex() const { return _poleIndex; } + glm::vec3 getPoleReferenceVector() const { return _poleReferenceVector; } + bool getPoleVectorEnabled() const { return _poleVectorEnabled; } int getIndex() const { return _index; } Type getType() const { return _type; } void setPose(const glm::quat& rotation, const glm::vec3& translation); void setPoleVector(const glm::vec3& poleVector) { _poleVector = poleVector; } - void setPoleIndex(int poleIndex) { _poleIndex = poleIndex; } + void setPoleReferenceVector(const glm::vec3& poleReferenceVector) { _poleReferenceVector = poleReferenceVector; } + void setPoleVectorEnabled(bool poleVectorEnabled) { _poleVectorEnabled = poleVectorEnabled; } void setIndex(int index) { _index = index; } void setType(int); void setFlexCoefficients(size_t numFlexCoefficientsIn, const float* flexCoefficientsIn); @@ -51,7 +53,8 @@ public: private: AnimPose _pose; glm::vec3 _poleVector; - int _poleIndex { -1 }; + glm::vec3 _poleReferenceVector; + bool _poleVectorEnabled { false }; int _index { -1 }; Type _type { Type::RotationAndPosition }; float _weight; diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 2966e882d2..1e6965faf5 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1261,6 +1261,8 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f } } + const float POLE_VECTOR_BLEND_FACTOR = 0.9f; + if (params.isLeftFootEnabled) { _animVars.set("leftFootPosition", params.leftFootPosition); _animVars.set("leftFootRotation", params.leftFootOrientation); @@ -1274,16 +1276,16 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f _prevLeftFootPoleVectorValid = true; _prevLeftFootPoleVector = poleVector; } - const float POLE_VECTOR_BLEND_FACTOR = 0.9f; _prevLeftFootPoleVector = lerp(poleVector, _prevLeftFootPoleVector, POLE_VECTOR_BLEND_FACTOR); + _animVars.set("leftFootPoleVectorEnabled", true); + _animVars.set("leftFootPoleReferenceVector", Vectors::UNIT_Z); _animVars.set("leftFootPoleVector", _prevLeftFootPoleVector); - _animVars.set("leftFootPoleJointName", QString("LeftFoot")); - } else { _animVars.unset("leftFootPosition"); _animVars.unset("leftFootRotation"); _animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition); + _animVars.set("leftFootPoleVectorEnabled", false); _prevLeftFootPoleVectorValid = false; } @@ -1300,14 +1302,15 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f _prevRightFootPoleVectorValid = true; _prevRightFootPoleVector = poleVector; } - const float POLE_VECTOR_BLEND_FACTOR = 0.9f; _prevRightFootPoleVector = lerp(poleVector, _prevRightFootPoleVector, POLE_VECTOR_BLEND_FACTOR); + _animVars.set("rightFootPoleVectorEnabled", true); + _animVars.set("rightFootPoleReferenceVector", Vectors::UNIT_Z); _animVars.set("rightFootPoleVector", _prevRightFootPoleVector); - _animVars.set("rightFootPoleJointName", QString("RightFoot")); } else { _animVars.unset("rightFootPosition"); _animVars.unset("rightFootRotation"); + _animVars.set("rightFootPoleVectorEnabled", false); _animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition); } } @@ -1497,16 +1500,16 @@ void Rig::computeAvatarBoundingCapsule( ikNode.setTargetVars("LeftHand", "leftHandPosition", "leftHandRotation", "leftHandType", "leftHandWeight", 1.0f, {}, - "leftHandPoleJointName", "leftHandPoleVector"); + QString(), QString(), QString()); ikNode.setTargetVars("RightHand", "rightHandPosition", "rightHandRotation", "rightHandType", "rightHandWeight", 1.0f, {}, - "rightHandPoleJointName", "rightHandPoleVector"); + QString(), QString(), QString()); ikNode.setTargetVars("LeftFoot", "leftFootPosition", "leftFootRotation", "leftFootType", "leftFootWeight", 1.0f, {}, - "leftFootPoleJointName", "leftFootPoleVector"); + QString(), QString(), QString()); ikNode.setTargetVars("RightFoot", "rightFootPosition", "rightFootRotation", "rightFootType", "rightFootWeight", 1.0f, {}, - "rightFootPoleJointName", "rightFootPoleVector"); + QString(), QString(), QString()); AnimPose geometryToRig = _modelOffset * _geometryOffset; From fac21033e738890368ce485cc90f192d7d86c9bd Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Wed, 14 Jun 2017 11:53:54 -0700 Subject: [PATCH 09/42] Fixed matthew sitting pose by opening up UpLeg twist constraint to +- 90 degrees. --- libraries/animation/src/AnimInverseKinematics.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 2a493a96ab..eb256497b7 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -1137,7 +1137,7 @@ void AnimInverseKinematics::initConstraints() { } else if (0 == baseName.compare("UpLeg", Qt::CaseSensitive)) { SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot()); - stConstraint->setTwistLimits(-PI / 4.0f, PI / 4.0f); + stConstraint->setTwistLimits(-PI / 2.0f, PI / 2.0f); std::vector swungDirections; float deltaTheta = PI / 4.0f; From cdfba52488f5cea72a71968d5721ccb1fca5c819 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 15 Jun 2017 15:02:42 -0700 Subject: [PATCH 10/42] precondition initial solution before solving to reduce limb locking. --- .../animation/src/AnimInverseKinematics.cpp | 54 +++++++++++++++++-- .../animation/src/AnimInverseKinematics.h | 1 + libraries/animation/src/AnimSkeleton.cpp | 6 +-- libraries/animation/src/AnimSkeleton.h | 2 +- 4 files changed, 56 insertions(+), 7 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index eb256497b7..f55537b3d6 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -394,6 +394,7 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const const float MIN_AXIS_LENGTH = 1.0e-4f; RotationConstraint* constraint = getConstraint(pivotIndex); + // only allow swing on lowerSpine if there is a hips IK target. if (_hipsTargetIndex < 0 && constraint && constraint->isLowerSpine() && tipIndex != _headIndex) { // for these types of targets we only allow twist at the lower-spine @@ -419,6 +420,7 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const float cosAngle = glm::clamp(glm::dot(leverArm, targetLine) / (glm::length(leverArm) * glm::length(targetLine)), -1.0f, 1.0f); float angle = acosf(cosAngle); const float MIN_ADJUSTMENT_ANGLE = 1.0e-4f; + if (angle > MIN_ADJUSTMENT_ANGLE) { // reduce angle by a flexCoefficient angle *= target.getFlexCoefficient(chainDepth); @@ -791,6 +793,7 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar return _relativePoses; } + //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 @@ -904,6 +907,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars { PROFILE_RANGE_EX(simulation_animation, "ik/ccd", 0xffff00ff, 0); + preconditionRelativePosesToAvoidLimbLock(context, targets); solve(context, targets); } @@ -1279,7 +1283,7 @@ void AnimInverseKinematics::initConstraints() { // we determine the max/min angles by rotating the swing limit lines from parent- to child-frame // then measure the angles to swing the yAxis into alignment glm::vec3 hingeAxis = - mirror * Vectors::UNIT_Z; - const float MIN_ELBOW_ANGLE = 0.05f; // ~2.8 deg (ajt-rad-to-deg 0.05) + const float MIN_ELBOW_ANGLE = 0.0f; const float MAX_ELBOW_ANGLE = 11.0f * PI / 12.0f; glm::quat invReferenceRotation = glm::inverse(referenceRotation); glm::vec3 minSwingAxis = invReferenceRotation * glm::angleAxis(MIN_ELBOW_ANGLE, hingeAxis) * Vectors::UNIT_Y; @@ -1310,7 +1314,7 @@ void AnimInverseKinematics::initConstraints() { // we determine the max/min angles by rotating the swing limit lines from parent- to child-frame // then measure the angles to swing the yAxis into alignment - const float MIN_KNEE_ANGLE = 0.097f; // ~5 deg + const float MIN_KNEE_ANGLE = 0.0f; const float MAX_KNEE_ANGLE = 7.0f * PI / 8.0f; // 157.5 deg glm::quat invReferenceRotation = glm::inverse(referenceRotation); glm::vec3 minSwingAxis = invReferenceRotation * glm::angleAxis(MIN_KNEE_ANGLE, hingeAxis) * Vectors::UNIT_Y; @@ -1659,6 +1663,50 @@ void AnimInverseKinematics::blendToPoses(const AnimPoseVec& targetPoses, const A } } +void AnimInverseKinematics::preconditionRelativePosesToAvoidLimbLock(const AnimContext& context, const std::vector& targets) { + const int NUM_LIMBS = 4; + std::pair limbs[NUM_LIMBS] = { + {_skeleton->nameToJointIndex("LeftHand"), _skeleton->nameToJointIndex("LeftArm")}, + {_skeleton->nameToJointIndex("RightHand"), _skeleton->nameToJointIndex("RightArm")}, + {_skeleton->nameToJointIndex("LeftFoot"), _skeleton->nameToJointIndex("LeftUpLeg")}, + {_skeleton->nameToJointIndex("RightFoot"), _skeleton->nameToJointIndex("RightUpLeg")} + }; + const float MIN_AXIS_LENGTH = 1.0e-4f; + + for (auto& target : targets) { + if (target.getIndex() != -1) { + for (int i = 0; i < NUM_LIMBS; i++) { + if (limbs[i].first == target.getIndex()) { + int tipIndex = limbs[i].first; + int baseIndex = limbs[i].second; + + // TODO: as an optimization, these poses can be computed in one pass down the chain, instead of three. + AnimPose tipPose = _skeleton->getAbsolutePose(tipIndex, _relativePoses); + AnimPose basePose = _skeleton->getAbsolutePose(baseIndex, _relativePoses); + AnimPose baseParentPose = _skeleton->getAbsolutePose(_skeleton->getParentIndex(baseIndex), _relativePoses); + + // to help reduce limb locking, and to help the CCD solver converge faster + // rotate the limbs leverArm over the targetLine. + glm::vec3 targetLine = target.getTranslation() - basePose.trans(); + glm::vec3 leverArm = tipPose.trans() - basePose.trans(); + glm::vec3 axis = glm::cross(leverArm, targetLine); + float axisLength = glm::length(axis); + if (axisLength > MIN_AXIS_LENGTH) { + // compute angle of rotation that brings tip to target + axis /= axisLength; + float cosAngle = glm::clamp(glm::dot(leverArm, targetLine) / (glm::length(leverArm) * glm::length(targetLine)), -1.0f, 1.0f); + float angle = acosf(cosAngle); + glm::quat newBaseRotation = glm::angleAxis(angle, axis) * basePose.rot(); + + // convert base rotation into relative space of base. + _relativePoses[baseIndex].rot() = glm::inverse(baseParentPose.rot()) * newBaseRotation; + } + } + } + } + } +} + void AnimInverseKinematics::initRelativePosesFromSolutionSource(SolutionSource solutionSource, const AnimPoseVec& underPoses) { const float RELAX_BLEND_FACTOR = (1.0f / 16.0f); const float COPY_BLEND_FACTOR = 1.0f; @@ -1678,7 +1726,7 @@ void AnimInverseKinematics::initRelativePosesFromSolutionSource(SolutionSource s break; case SolutionSource::LimitCenterPoses: // essentially copy limitCenterPoses over to _relativePoses. - blendToPoses(_limitCenterPoses, underPoses, COPY_BLEND_FACTOR); + blendToPoses(underPoses, _limitCenterPoses, COPY_BLEND_FACTOR); break; } } diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index 016317aee3..d473ae3698 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -82,6 +82,7 @@ protected: void debugDrawSpineSplines(const AnimContext& context, const std::vector& targets) const; void initRelativePosesFromSolutionSource(SolutionSource solutionSource, const AnimPoseVec& underPose); void blendToPoses(const AnimPoseVec& targetPoses, const AnimPoseVec& underPose, float blendFactor); + void preconditionRelativePosesToAvoidLimbLock(const AnimContext& context, const std::vector& targets); // used to pre-compute information about each joint influeced by a spline IK target. struct SplineJointInfo { diff --git a/libraries/animation/src/AnimSkeleton.cpp b/libraries/animation/src/AnimSkeleton.cpp index 350fe8a534..062e016660 100644 --- a/libraries/animation/src/AnimSkeleton.cpp +++ b/libraries/animation/src/AnimSkeleton.cpp @@ -76,11 +76,11 @@ const QString& AnimSkeleton::getJointName(int jointIndex) const { return _joints[jointIndex].name; } -AnimPose AnimSkeleton::getAbsolutePose(int jointIndex, const AnimPoseVec& poses) const { - if (jointIndex < 0 || jointIndex >= (int)poses.size() || jointIndex >= _jointsSize) { +AnimPose AnimSkeleton::getAbsolutePose(int jointIndex, const AnimPoseVec& relativePoses) const { + if (jointIndex < 0 || jointIndex >= (int)relativePoses.size() || jointIndex >= _jointsSize) { return AnimPose::identity; } else { - return getAbsolutePose(_joints[jointIndex].parentIndex, poses) * poses[jointIndex]; + return getAbsolutePose(_joints[jointIndex].parentIndex, relativePoses) * relativePoses[jointIndex]; } } diff --git a/libraries/animation/src/AnimSkeleton.h b/libraries/animation/src/AnimSkeleton.h index 0988c26bdb..6315f2d62b 100644 --- a/libraries/animation/src/AnimSkeleton.h +++ b/libraries/animation/src/AnimSkeleton.h @@ -50,7 +50,7 @@ public: int getParentIndex(int jointIndex) const; - AnimPose getAbsolutePose(int jointIndex, const AnimPoseVec& poses) const; + AnimPose getAbsolutePose(int jointIndex, const AnimPoseVec& relativePoses) const; void convertRelativePosesToAbsolute(AnimPoseVec& poses) const; void convertAbsolutePosesToRelative(AnimPoseVec& poses) const; From e7991579ef6f9021cb01cf5f3aee5681c8cdeec1 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 16 Jun 2017 17:29:56 -0700 Subject: [PATCH 11/42] Enabled elbow pole vectors There are still some issues with rotations of the elbow pole vectors. * When the (hand - shoulder) vector approaches the normal vector used in Rig::calculateElbowPoleVector() unexpected twists can occur. * Also, when the (hand - shoulder) vector approaches zero, the IK system starts to flutter between two states. * The shoulder twist constraint probably needs to be opened up for more natural range of motion. --- .../resources/avatar/avatar-animation.json | 10 +- .../animation/src/AnimInverseKinematics.cpp | 2 +- libraries/animation/src/Rig.cpp | 95 ++++++++++++++++++- libraries/animation/src/Rig.h | 9 +- 4 files changed, 110 insertions(+), 6 deletions(-) diff --git a/interface/resources/avatar/avatar-animation.json b/interface/resources/avatar/avatar-animation.json index c9552a3ead..018987b58b 100644 --- a/interface/resources/avatar/avatar-animation.json +++ b/interface/resources/avatar/avatar-animation.json @@ -68,7 +68,10 @@ "typeVar": "rightHandType", "weightVar": "rightHandWeight", "weight": 1.0, - "flexCoefficients": [1, 0.5, 0.5, 0.2, 0.01, 0.005, 0.001, 0.0, 0.0] + "flexCoefficients": [1, 0.5, 0.5, 0.2, 0.01, 0.005, 0.001, 0.0, 0.0], + "poleVectorEnabledVar": "rightHandPoleVectorEnabled", + "poleReferenceVectorVar": "rightHandPoleReferenceVector", + "poleVectorVar": "rightHandPoleVector" }, { "jointName": "LeftHand", @@ -77,7 +80,10 @@ "typeVar": "leftHandType", "weightVar": "leftHandWeight", "weight": 1.0, - "flexCoefficients": [1, 0.5, 0.5, 0.2, 0.01, 0.005, 0.001, 0.0, 0.0] + "flexCoefficients": [1, 0.5, 0.5, 0.2, 0.01, 0.005, 0.001, 0.0, 0.0], + "poleVectorEnabledVar": "leftHandPoleVectorEnabled", + "poleReferenceVectorVar": "leftHandPoleReferenceVector", + "poleVectorVar": "leftHandPoleVector" }, { "jointName": "RightFoot", diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index f55537b3d6..080e6449d9 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -42,7 +42,7 @@ static void lookupJointChainInfo(AnimInverseKinematics::JointChainInfo* jointCha } } -float easeOutExpo(float t) { +static float easeOutExpo(float t) { return 1.0f - powf(2, -10.0f * t); } diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 1e6965faf5..99678eaa28 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1140,6 +1140,47 @@ void Rig::updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm } } +static float easeOutExpo(float t) { + return 1.0f - powf(2, -10.0f * t); +} + +static glm::quat quatLerp(const glm::quat& q1, const glm::quat& q2, float alpha) { + float dot = glm::dot(q1, q2); + glm::quat temp; + if (dot < 0.0f) { + temp = -q2; + } else { + temp = q2; + } + return glm::normalize(glm::lerp(q1, temp, alpha)); +} + +glm::vec3 Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, int hipsIndex, bool isLeft) const { + AnimPose hipsPose = _externalPoseSet._absolutePoses[hipsIndex]; + AnimPose handPose = _externalPoseSet._absolutePoses[handIndex]; + AnimPose elbowPose = _externalPoseSet._absolutePoses[elbowIndex]; + AnimPose armPose = _externalPoseSet._absolutePoses[armIndex]; + glm::vec3 d = glm::normalize(handPose.trans() - armPose.trans()); + + glm::quat elbowToHandDelta = handPose.rot() * glm::inverse(elbowPose.rot()); + const float WRIST_POLE_ADJUST_FACTOR = 0.5f; + glm::quat poleAdjust = quatLerp(Quaternions::IDENTITY, elbowToHandDelta, WRIST_POLE_ADJUST_FACTOR); + + glm::vec3 n; + if (isLeft) { + n = hipsPose.rot() * glm::normalize(glm::vec3(1.0f, 1.5f, -0.1f)); + } else { + n = hipsPose.rot() * -glm::normalize(glm::vec3(-1.0f, 1.5f, -0.1f)); + } + + // project d onto n. + glm::vec3 dProj = d - glm::dot(d, n) * n; + glm::vec3 dProjRot = glm::angleAxis(PI / 2.0f, n) * dProj; + glm::vec3 pole = glm::normalize(dProjRot); + + return glm::normalize(poleAdjust * pole); +} + glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, const glm::quat& footTargetOrientation, int hipsIndex) const { AnimPose defaultPose = _animSkeleton->getAbsoluteDefaultPose(footJointIndex); @@ -1175,6 +1216,8 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f const float RELAX_DURATION = 0.6f; + const float POLE_VECTOR_BLEND_FACTOR = 0.9f; + if (params.isLeftEnabled) { glm::vec3 handPosition = params.leftPosition; if (!bodySensorTrackingEnabled) { @@ -1191,7 +1234,32 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f _isLeftHandControlled = true; _lastLeftHandControlledPose = AnimPose(glm::vec3(1.0f), params.leftOrientation, handPosition); + + // compute pole vector + int handJointIndex = _animSkeleton->nameToJointIndex("LeftHand"); + int armJointIndex = _animSkeleton->nameToJointIndex("LeftArm"); + int elbowJointIndex = _animSkeleton->nameToJointIndex("LeftForeArm"); + if (elbowJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0) { + glm::vec3 poleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, hipsIndex, true); + + // smooth toward desired pole vector from previous pole vector... to reduce jitter + if (!_prevLeftHandPoleVectorValid) { + _prevLeftHandPoleVectorValid = true; + _prevLeftHandPoleVector = poleVector; + } + _prevLeftHandPoleVector = lerp(poleVector, _prevLeftHandPoleVector, POLE_VECTOR_BLEND_FACTOR); + + _animVars.set("leftHandPoleVectorEnabled", true); + _animVars.set("leftHandPoleReferenceVector", Vectors::UNIT_X); + _animVars.set("leftHandPoleVector", _prevLeftHandPoleVector); + } else { + _prevLeftHandPoleVectorValid = false; + _animVars.set("leftHandPoleVectorEnabled", false); + } } else { + _prevLeftHandPoleVectorValid = false; + _animVars.set("leftHandPoleVectorEnabled", false); + if (_isLeftHandControlled) { _leftHandRelaxDuration = RELAX_DURATION; _isLeftHandControlled = false; @@ -1234,7 +1302,32 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f _isRightHandControlled = true; _lastRightHandControlledPose = AnimPose(glm::vec3(1.0f), params.rightOrientation, handPosition); + + // compute pole vector + int handJointIndex = _animSkeleton->nameToJointIndex("RightHand"); + int armJointIndex = _animSkeleton->nameToJointIndex("RightArm"); + int elbowJointIndex = _animSkeleton->nameToJointIndex("RightForeArm"); + if (elbowJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0) { + glm::vec3 poleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, hipsIndex, false); + + // smooth toward desired pole vector from previous pole vector... to reduce jitter + if (!_prevRightHandPoleVectorValid) { + _prevRightHandPoleVectorValid = true; + _prevRightHandPoleVector = poleVector; + } + _prevRightHandPoleVector = lerp(poleVector, _prevRightHandPoleVector, POLE_VECTOR_BLEND_FACTOR); + + _animVars.set("rightHandPoleVectorEnabled", true); + _animVars.set("rightHandPoleReferenceVector", -Vectors::UNIT_X); + _animVars.set("rightHandPoleVector", _prevRightHandPoleVector); + } else { + _prevRightHandPoleVectorValid = false; + _animVars.set("rightHandPoleVectorEnabled", false); + } } else { + _prevRightHandPoleVectorValid = false; + _animVars.set("rightHandPoleVectorEnabled", false); + if (_isRightHandControlled) { _rightHandRelaxDuration = RELAX_DURATION; _isRightHandControlled = false; @@ -1261,8 +1354,6 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f } } - const float POLE_VECTOR_BLEND_FACTOR = 0.9f; - if (params.isLeftFootEnabled) { _animVars.set("leftFootPosition", params.leftFootPosition); _animVars.set("leftFootRotation", params.leftFootOrientation); diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 4fc5f76146..06822d666e 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -249,7 +249,8 @@ protected: void updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm::quat& modelRotation, const glm::vec3& lookAt, const glm::vec3& saccade); void calcAnimAlpha(float speed, const std::vector& referenceSpeeds, float* alphaOut) const; - glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, const glm::quat& footTargetOrientation, int hipsIndex) const; + glm::vec3 calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, int hipsIndex, bool isLeft) const; + glm::vec3 calculateKneePoleVector(int footJointIndex, const glm::quat& footTargetOrientation, int hipsIndex) const; AnimPose _modelOffset; // model to rig space AnimPose _geometryOffset; // geometry to model space (includes unit offset & fst offsets) @@ -362,6 +363,12 @@ protected: glm::vec3 _prevLeftFootPoleVector = { Vectors::UNIT_Z }; bool _prevLeftFootPoleVectorValid = { false }; + + glm::vec3 _prevRightHandPoleVector = { -Vectors::UNIT_Z }; + bool _prevRightHandPoleVectorValid = { false }; + + glm::vec3 _prevLeftHandPoleVector = { -Vectors::UNIT_Z }; + bool _prevLeftHandPoleVectorValid = { false }; }; #endif /* defined(__hifi__Rig__) */ From 81852cd91c744e81f9296d09ec04f9b8590d6fcc Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 16 Jun 2017 18:15:41 -0700 Subject: [PATCH 12/42] warning fixes --- libraries/animation/src/AnimInverseKinematics.cpp | 2 -- libraries/animation/src/Rig.cpp | 4 ---- 2 files changed, 6 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 080e6449d9..3ea8937eec 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -565,8 +565,6 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const glm::vec3 p = target.getPoleVector(); glm::vec3 eProj = e - glm::dot(e, dUnit) * dUnit; glm::vec3 pProj = p - glm::dot(p, dUnit) * dUnit; - float eProjLen = glm::length(eProj); - float pProjLen = glm::length(pProj); const float PROJ_VECTOR_LEN = 10.0f; const float POLE_VECTOR_LEN = 100.0f; diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 5ad664614e..43ce209420 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1140,10 +1140,6 @@ void Rig::updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm } } -static float easeOutExpo(float t) { - return 1.0f - powf(2, -10.0f * t); -} - static glm::quat quatLerp(const glm::quat& q1, const glm::quat& q2, float alpha) { float dot = glm::dot(q1, q2); glm::quat temp; From 3f5aba26557be43e31c98c73d395ca028ca0dbd6 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 19 Jun 2017 13:21:12 -0700 Subject: [PATCH 13/42] improved elbow pole vector calculation Also, pole vectors are blended spherical linearly, this might help fast moving pole vectors from rotating too quickly. --- libraries/animation/src/Rig.cpp | 57 ++++++++++++++++++++------------- 1 file changed, 35 insertions(+), 22 deletions(-) diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 43ce209420..b1d0dca9e6 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1158,23 +1158,27 @@ glm::vec3 Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIn AnimPose armPose = _externalPoseSet._absolutePoses[armIndex]; glm::vec3 d = glm::normalize(handPose.trans() - armPose.trans()); + float sign = isLeft ? 1.0f : -1.0f; + + glm::vec3 n = hipsPose.rot() * Vectors::UNIT_X; + glm::vec3 y = hipsPose.rot() * Vectors::UNIT_Y; + // project d onto n. + glm::vec3 dProj = d - glm::dot(d, n) * n; + const float LATERAL_OFFSET = 0.333f; + const float VERTICAL_OFFSET = -0.333f; + + // give dProj a bit of offset away from the body. + glm::vec3 dProjWithOffset = dProj + sign * LATERAL_OFFSET * n + y * VERTICAL_OFFSET; + + // rotate dProj by 90 degrees to get the poleVector. + glm::vec3 poleVector = glm::angleAxis(PI / 2.0f, n) * dProjWithOffset; + + // blend the wrist oreintation into the pole vector to reduce the painfully bent wrist problem. glm::quat elbowToHandDelta = handPose.rot() * glm::inverse(elbowPose.rot()); const float WRIST_POLE_ADJUST_FACTOR = 0.5f; glm::quat poleAdjust = quatLerp(Quaternions::IDENTITY, elbowToHandDelta, WRIST_POLE_ADJUST_FACTOR); - glm::vec3 n; - if (isLeft) { - n = hipsPose.rot() * glm::normalize(glm::vec3(1.0f, 1.5f, -0.1f)); - } else { - n = hipsPose.rot() * -glm::normalize(glm::vec3(-1.0f, 1.5f, -0.1f)); - } - - // project d onto n. - glm::vec3 dProj = d - glm::dot(d, n) * n; - glm::vec3 dProjRot = glm::angleAxis(PI / 2.0f, n) * dProj; - glm::vec3 pole = glm::normalize(dProjRot); - - return glm::normalize(poleAdjust * pole); + return glm::normalize(poleAdjust * poleVector); } glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, const glm::quat& footTargetOrientation, int hipsIndex) const { @@ -1217,7 +1221,8 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f const bool LEFT_HAND = true; const bool RIGHT_HAND = false; - const float POLE_VECTOR_BLEND_FACTOR = 0.9f; + const float KNEE_POLE_VECTOR_BLEND_FACTOR = 0.9f; + const float ELBOW_POLE_VECTOR_BLEND_FACTOR = 0.9f; if (params.isLeftEnabled) { if (!_isLeftHandControlled) { @@ -1232,8 +1237,8 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f // Move hand from non-controlled position to controlled position. _leftHandControlTimeRemaining = std::max(_leftHandControlTimeRemaining - dt, 0.0f); AnimPose handPose(Vectors::ONE, handRotation, handPosition); - if (transitionHandPose(_leftHandControlTimeRemaining, CONTROL_DURATION, handPose, LEFT_HAND, TO_CONTROLLED, - handPose)) { + if (transitionHandPose(_leftHandControlTimeRemaining, CONTROL_DURATION, handPose, + LEFT_HAND, TO_CONTROLLED, handPose)) { handPosition = handPose.trans(); handRotation = handPose.rot(); } @@ -1268,7 +1273,9 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f _prevLeftHandPoleVectorValid = true; _prevLeftHandPoleVector = poleVector; } - _prevLeftHandPoleVector = lerp(poleVector, _prevLeftHandPoleVector, POLE_VECTOR_BLEND_FACTOR); + glm::quat deltaRot = rotationBetween(_prevLeftHandPoleVector, poleVector); + glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, ELBOW_POLE_VECTOR_BLEND_FACTOR); + _prevLeftHandPoleVector = smoothDeltaRot * _prevLeftHandPoleVector; _animVars.set("leftHandPoleVectorEnabled", true); _animVars.set("leftHandPoleReferenceVector", Vectors::UNIT_X); @@ -1290,8 +1297,8 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f // Move hand from controlled position to non-controlled position. _leftHandRelaxTimeRemaining = std::max(_leftHandRelaxTimeRemaining - dt, 0.0f); AnimPose handPose; - if (transitionHandPose(_leftHandRelaxTimeRemaining, RELAX_DURATION, _lastLeftHandControlledPose, LEFT_HAND, - FROM_CONTROLLED, handPose)) { + if (transitionHandPose(_leftHandRelaxTimeRemaining, RELAX_DURATION, _lastLeftHandControlledPose, + LEFT_HAND, FROM_CONTROLLED, handPose)) { _animVars.set("leftHandPosition", handPose.trans()); _animVars.set("leftHandRotation", handPose.rot()); _animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition); @@ -1352,7 +1359,9 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f _prevRightHandPoleVectorValid = true; _prevRightHandPoleVector = poleVector; } - _prevRightHandPoleVector = lerp(poleVector, _prevRightHandPoleVector, POLE_VECTOR_BLEND_FACTOR); + glm::quat deltaRot = rotationBetween(_prevRightHandPoleVector, poleVector); + glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, ELBOW_POLE_VECTOR_BLEND_FACTOR); + _prevRightHandPoleVector = smoothDeltaRot * _prevRightHandPoleVector; _animVars.set("rightHandPoleVectorEnabled", true); _animVars.set("rightHandPoleReferenceVector", -Vectors::UNIT_X); @@ -1401,7 +1410,9 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f _prevLeftFootPoleVectorValid = true; _prevLeftFootPoleVector = poleVector; } - _prevLeftFootPoleVector = lerp(poleVector, _prevLeftFootPoleVector, POLE_VECTOR_BLEND_FACTOR); + glm::quat deltaRot = rotationBetween(_prevLeftFootPoleVector, poleVector); + glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, KNEE_POLE_VECTOR_BLEND_FACTOR); + _prevLeftFootPoleVector = smoothDeltaRot * _prevLeftFootPoleVector; _animVars.set("leftFootPoleVectorEnabled", true); _animVars.set("leftFootPoleReferenceVector", Vectors::UNIT_Z); @@ -1427,7 +1438,9 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f _prevRightFootPoleVectorValid = true; _prevRightFootPoleVector = poleVector; } - _prevRightFootPoleVector = lerp(poleVector, _prevRightFootPoleVector, POLE_VECTOR_BLEND_FACTOR); + glm::quat deltaRot = rotationBetween(_prevRightFootPoleVector, poleVector); + glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, KNEE_POLE_VECTOR_BLEND_FACTOR); + _prevRightFootPoleVector = smoothDeltaRot * _prevRightFootPoleVector; _animVars.set("rightFootPoleVectorEnabled", true); _animVars.set("rightFootPoleReferenceVector", Vectors::UNIT_Z); From 6bbc5bfbea17c16fde201d2a655aacdaf17e4617 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 19 Jun 2017 13:42:39 -0700 Subject: [PATCH 14/42] formatting on initializers --- libraries/animation/src/Rig.h | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index c9033b6e6e..d101425a77 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -351,8 +351,8 @@ protected: int _nextStateHandlerId { 0 }; QMutex _stateMutex; - bool transitionHandPose(float deltaTime, float totalDuration, AnimPose& controlledHandPose, bool isLeftHand, - bool isToControlled, AnimPose& returnHandPose); + bool transitionHandPose(float deltaTime, float totalDuration, AnimPose& controlledHandPose, bool isLeftHand, + bool isToControlled, AnimPose& returnHandPose); bool _isLeftHandControlled { false }; bool _isRightHandControlled { false }; @@ -363,17 +363,17 @@ protected: AnimPose _lastLeftHandControlledPose; AnimPose _lastRightHandControlledPose; - glm::vec3 _prevRightFootPoleVector = { Vectors::UNIT_Z }; - bool _prevRightFootPoleVectorValid = { false }; + glm::vec3 _prevRightFootPoleVector { Vectors::UNIT_Z }; + bool _prevRightFootPoleVectorValid { false }; - glm::vec3 _prevLeftFootPoleVector = { Vectors::UNIT_Z }; - bool _prevLeftFootPoleVectorValid = { false }; + glm::vec3 _prevLeftFootPoleVector { Vectors::UNIT_Z }; + bool _prevLeftFootPoleVectorValid { false }; - glm::vec3 _prevRightHandPoleVector = { -Vectors::UNIT_Z }; - bool _prevRightHandPoleVectorValid = { false }; + glm::vec3 _prevRightHandPoleVector { -Vectors::UNIT_Z }; + bool _prevRightHandPoleVectorValid { false }; - glm::vec3 _prevLeftHandPoleVector = { -Vectors::UNIT_Z }; - bool _prevLeftHandPoleVectorValid = { false }; + glm::vec3 _prevLeftHandPoleVector { -Vectors::UNIT_Z }; + bool _prevLeftHandPoleVectorValid { false }; }; #endif /* defined(__hifi__Rig__) */ From c236afe68c9ccf08ffed40f81e95d38f2db9300d Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 19 Jun 2017 16:54:39 -0700 Subject: [PATCH 15/42] Simplify passing data from MySkeletonModel to Rig --- interface/src/avatar/MySkeletonModel.cpp | 125 ++--- libraries/animation/src/AnimPose.h | 2 + libraries/animation/src/Rig.cpp | 607 ++++++++++++----------- libraries/animation/src/Rig.h | 63 +-- 4 files changed, 407 insertions(+), 390 deletions(-) diff --git a/interface/src/avatar/MySkeletonModel.cpp b/interface/src/avatar/MySkeletonModel.cpp index e74df4cf0f..89b6c36c63 100644 --- a/interface/src/avatar/MySkeletonModel.cpp +++ b/interface/src/avatar/MySkeletonModel.cpp @@ -47,110 +47,113 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { MyAvatar* myAvatar = static_cast(_owningAvatar); - Rig::HeadParameters headParams; + Rig::ControllerParameters params; + + AnimPose avatarToRigPose(glm::vec3(1.0f), Quaternions::Y_180, glm::vec3(0.0f)); // input action is the highest priority source for head orientation. auto avatarHeadPose = myAvatar->getHeadControllerPoseInAvatarFrame(); if (avatarHeadPose.isValid()) { - glm::mat4 rigHeadMat = Matrices::Y_180 * - createMatFromQuatAndPos(avatarHeadPose.getRotation(), avatarHeadPose.getTranslation()); - headParams.rigHeadPosition = extractTranslation(rigHeadMat); - headParams.rigHeadOrientation = glmExtractRotation(rigHeadMat); - headParams.headEnabled = true; + AnimPose pose(avatarHeadPose.getRotation(), avatarHeadPose.getTranslation()); + params.controllerPoses[Rig::ControllerType_Head] = avatarToRigPose * pose; + params.controllerActiveFlags[Rig::ControllerType_Head] = true; } else { // even though full head IK is disabled, the rig still needs the head orientation to rotate the head up and // down in desktop mode. // preMult 180 is necessary to convert from avatar to rig coordinates. // postMult 180 is necessary to convert head from -z forward to z forward. - headParams.rigHeadOrientation = Quaternions::Y_180 * head->getFinalOrientationInLocalFrame() * Quaternions::Y_180; - headParams.headEnabled = false; + glm::quat headRot = Quaternions::Y_180 * head->getFinalOrientationInLocalFrame() * Quaternions::Y_180; + params.controllerPoses[Rig::ControllerType_Head] = AnimPose(glm::vec3(1.0f), headRot, glm::vec3(0.0f)); + params.controllerActiveFlags[Rig::ControllerType_Head] = false; } auto avatarHipsPose = myAvatar->getHipsControllerPoseInAvatarFrame(); if (avatarHipsPose.isValid()) { - glm::mat4 rigHipsMat = Matrices::Y_180 * createMatFromQuatAndPos(avatarHipsPose.getRotation(), avatarHipsPose.getTranslation()); - headParams.hipsMatrix = rigHipsMat; - headParams.hipsEnabled = true; + AnimPose pose(avatarHipsPose.getRotation(), avatarHipsPose.getTranslation()); + params.controllerPoses[Rig::ControllerType_Hips] = avatarToRigPose * pose; + params.controllerActiveFlags[Rig::ControllerType_Hips] = true; } else { - headParams.hipsEnabled = false; + params.controllerPoses[Rig::ControllerType_Hips] = AnimPose::identity; + params.controllerActiveFlags[Rig::ControllerType_Hips] = false; } auto avatarSpine2Pose = myAvatar->getSpine2ControllerPoseInAvatarFrame(); if (avatarSpine2Pose.isValid()) { - glm::mat4 rigSpine2Mat = Matrices::Y_180 * createMatFromQuatAndPos(avatarSpine2Pose.getRotation(), avatarSpine2Pose.getTranslation()); - headParams.spine2Matrix = rigSpine2Mat; - headParams.spine2Enabled = true; + AnimPose pose(avatarSpine2Pose.getRotation(), avatarSpine2Pose.getTranslation()); + params.controllerPoses[Rig::ControllerType_Spine2] = avatarToRigPose * pose; + params.controllerActiveFlags[Rig::ControllerType_Spine2] = true; } else { - headParams.spine2Enabled = false; + params.controllerPoses[Rig::ControllerType_Spine2] = AnimPose::identity; + params.controllerActiveFlags[Rig::ControllerType_Spine2] = false; } auto avatarRightArmPose = myAvatar->getRightArmControllerPoseInAvatarFrame(); if (avatarRightArmPose.isValid()) { - glm::mat4 rightArmMat = Matrices::Y_180 * createMatFromQuatAndPos(avatarRightArmPose.getRotation(), avatarRightArmPose.getTranslation()); - headParams.rightArmPosition = extractTranslation(rightArmMat); - headParams.rightArmRotation = glmExtractRotation(rightArmMat); - headParams.rightArmEnabled = true; + AnimPose pose(avatarRightArmPose.getRotation(), avatarRightArmPose.getTranslation()); + params.controllerPoses[Rig::ControllerType_RightArm] = avatarToRigPose * pose; + params.controllerActiveFlags[Rig::ControllerType_RightArm] = true; } else { - headParams.rightArmEnabled = false; + params.controllerPoses[Rig::ControllerType_RightArm] = AnimPose::identity; + params.controllerActiveFlags[Rig::ControllerType_RightArm] = false; } - + auto avatarLeftArmPose = myAvatar->getLeftArmControllerPoseInAvatarFrame(); if (avatarLeftArmPose.isValid()) { - glm::mat4 leftArmMat = Matrices::Y_180 * createMatFromQuatAndPos(avatarLeftArmPose.getRotation(), avatarLeftArmPose.getTranslation()); - headParams.leftArmPosition = extractTranslation(leftArmMat); - headParams.leftArmRotation = glmExtractRotation(leftArmMat); - headParams.leftArmEnabled = true; + AnimPose pose(avatarLeftArmPose.getRotation(), avatarLeftArmPose.getTranslation()); + params.controllerPoses[Rig::ControllerType_LeftArm] = avatarToRigPose * pose; + params.controllerActiveFlags[Rig::ControllerType_LeftArm] = true; } else { - headParams.leftArmEnabled = false; + params.controllerPoses[Rig::ControllerType_LeftArm] = AnimPose::identity; + params.controllerActiveFlags[Rig::ControllerType_LeftArm] = false; } - headParams.isTalking = head->getTimeWithoutTalking() <= 1.5f; - - _rig.updateFromHeadParameters(headParams, deltaTime); - - Rig::HandAndFeetParameters handAndFeetParams; - - auto leftPose = myAvatar->getLeftHandControllerPoseInAvatarFrame(); - if (leftPose.isValid()) { - handAndFeetParams.isLeftEnabled = true; - handAndFeetParams.leftPosition = Quaternions::Y_180 * leftPose.getTranslation(); - handAndFeetParams.leftOrientation = Quaternions::Y_180 * leftPose.getRotation(); + auto avatarLeftHandPose = myAvatar->getLeftHandControllerPoseInAvatarFrame(); + if (avatarLeftHandPose.isValid()) { + AnimPose pose(avatarLeftHandPose.getRotation(), avatarLeftHandPose.getTranslation()); + params.controllerPoses[Rig::ControllerType_LeftHand] = avatarToRigPose * pose; + params.controllerActiveFlags[Rig::ControllerType_LeftHand] = true; } else { - handAndFeetParams.isLeftEnabled = false; + params.controllerPoses[Rig::ControllerType_LeftHand] = AnimPose::identity; + params.controllerActiveFlags[Rig::ControllerType_LeftHand] = false; } - auto rightPose = myAvatar->getRightHandControllerPoseInAvatarFrame(); - if (rightPose.isValid()) { - handAndFeetParams.isRightEnabled = true; - handAndFeetParams.rightPosition = Quaternions::Y_180 * rightPose.getTranslation(); - handAndFeetParams.rightOrientation = Quaternions::Y_180 * rightPose.getRotation(); + auto avatarRightHandPose = myAvatar->getRightHandControllerPoseInAvatarFrame(); + if (avatarRightHandPose.isValid()) { + AnimPose pose(avatarRightHandPose.getRotation(), avatarRightHandPose.getTranslation()); + params.controllerPoses[Rig::ControllerType_RightHand] = avatarToRigPose * pose; + params.controllerActiveFlags[Rig::ControllerType_RightHand] = true; } else { - handAndFeetParams.isRightEnabled = false; + params.controllerPoses[Rig::ControllerType_RightHand] = AnimPose::identity; + params.controllerActiveFlags[Rig::ControllerType_RightHand] = false; } - auto leftFootPose = myAvatar->getLeftFootControllerPoseInAvatarFrame(); - if (leftFootPose.isValid()) { - handAndFeetParams.isLeftFootEnabled = true; - handAndFeetParams.leftFootPosition = Quaternions::Y_180 * leftFootPose.getTranslation(); - handAndFeetParams.leftFootOrientation = Quaternions::Y_180 * leftFootPose.getRotation(); + auto avatarLeftFootPose = myAvatar->getLeftFootControllerPoseInAvatarFrame(); + if (avatarLeftFootPose.isValid()) { + AnimPose pose(avatarLeftFootPose.getRotation(), avatarLeftFootPose.getTranslation()); + params.controllerPoses[Rig::ControllerType_LeftFoot] = avatarToRigPose * pose; + params.controllerActiveFlags[Rig::ControllerType_LeftFoot] = true; } else { - handAndFeetParams.isLeftFootEnabled = false; + params.controllerPoses[Rig::ControllerType_LeftFoot] = AnimPose::identity; + params.controllerActiveFlags[Rig::ControllerType_LeftFoot] = false; } - auto rightFootPose = myAvatar->getRightFootControllerPoseInAvatarFrame(); - if (rightFootPose.isValid()) { - handAndFeetParams.isRightFootEnabled = true; - handAndFeetParams.rightFootPosition = Quaternions::Y_180 * rightFootPose.getTranslation(); - handAndFeetParams.rightFootOrientation = Quaternions::Y_180 * rightFootPose.getRotation(); + auto avatarRightFootPose = myAvatar->getRightFootControllerPoseInAvatarFrame(); + if (avatarRightFootPose.isValid()) { + AnimPose pose(avatarRightFootPose.getRotation(), avatarRightFootPose.getTranslation()); + params.controllerPoses[Rig::ControllerType_RightFoot] = avatarToRigPose * pose; + params.controllerActiveFlags[Rig::ControllerType_RightFoot] = true; } else { - handAndFeetParams.isRightFootEnabled = false; + params.controllerPoses[Rig::ControllerType_RightFoot] = AnimPose::identity; + params.controllerActiveFlags[Rig::ControllerType_RightFoot] = false; } - handAndFeetParams.bodyCapsuleRadius = myAvatar->getCharacterController()->getCapsuleRadius(); - handAndFeetParams.bodyCapsuleHalfHeight = myAvatar->getCharacterController()->getCapsuleHalfHeight(); - handAndFeetParams.bodyCapsuleLocalOffset = myAvatar->getCharacterController()->getCapsuleLocalOffset(); + params.bodyCapsuleRadius = myAvatar->getCharacterController()->getCapsuleRadius(); + params.bodyCapsuleHalfHeight = myAvatar->getCharacterController()->getCapsuleHalfHeight(); + params.bodyCapsuleLocalOffset = myAvatar->getCharacterController()->getCapsuleLocalOffset(); - _rig.updateFromHandAndFeetParameters(handAndFeetParams, deltaTime); + params.isTalking = head->getTimeWithoutTalking() <= 1.5f; + + _rig.updateFromControllerParameters(params, deltaTime); Rig::CharacterControllerState ccState = convertCharacterControllerState(myAvatar->getCharacterController()->getState()); diff --git a/libraries/animation/src/AnimPose.h b/libraries/animation/src/AnimPose.h index a2e22a24be..2df3d1f2e4 100644 --- a/libraries/animation/src/AnimPose.h +++ b/libraries/animation/src/AnimPose.h @@ -21,6 +21,8 @@ class AnimPose { public: AnimPose() {} explicit AnimPose(const glm::mat4& mat); + explicit AnimPose(const glm::quat& rotIn) : _scale(1.0f), _rot(rotIn), _trans(0.0f) {} + AnimPose(const glm::quat& rotIn, const glm::vec3& transIn) : _scale(1.0f), _rot(rotIn), _trans(transIn) {} AnimPose(const glm::vec3& scaleIn, const glm::quat& rotIn, const glm::vec3& transIn) : _scale(scaleIn), _rot(rotIn), _trans(transIn) {} static const AnimPose identity; diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index b1d0dca9e6..0ee42372b1 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1010,46 +1010,6 @@ glm::quat Rig::getJointDefaultRotationInParentFrame(int jointIndex) { return glm::quat(); } -void Rig::updateFromHeadParameters(const HeadParameters& params, float dt) { - updateHeadAnimVars(params); - - _animVars.set("isTalking", params.isTalking); - _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); - } - - if (params.hipsEnabled && params.spine2Enabled) { - _animVars.set("spine2Type", (int)IKTarget::Type::Spline); - _animVars.set("spine2Position", extractTranslation(params.spine2Matrix)); - _animVars.set("spine2Rotation", glmExtractRotation(params.spine2Matrix)); - } else { - _animVars.set("spine2Type", (int)IKTarget::Type::Unknown); - } - - if (params.leftArmEnabled) { - _animVars.set("leftArmType", (int)IKTarget::Type::RotationAndPosition); - _animVars.set("leftArmPosition", params.leftArmPosition); - _animVars.set("leftArmRotation", params.leftArmRotation); - } else { - _animVars.set("leftArmType", (int)IKTarget::Type::Unknown); - } - - if (params.rightArmEnabled) { - _animVars.set("rightArmType", (int)IKTarget::Type::RotationAndPosition); - _animVars.set("rightArmPosition", params.rightArmPosition); - _animVars.set("rightArmRotation", params.rightArmRotation); - } else { - _animVars.set("rightArmType", (int)IKTarget::Type::Unknown); - } -} void Rig::updateFromEyeParameters(const EyeParameters& params) { updateEyeJoint(params.leftEyeJointIndex, params.modelTranslation, params.modelRotation, params.eyeLookAt, params.eyeSaccade); @@ -1081,12 +1041,12 @@ void Rig::computeHeadFromHMD(const AnimPose& hmdPose, glm::vec3& headPositionOut headOrientationOut = hmdOrientation; } -void Rig::updateHeadAnimVars(const HeadParameters& params) { +void Rig::updateHead(bool headEnabled, bool hipsEnabled, const AnimPose& headPose) { if (_animSkeleton) { - if (params.headEnabled) { - _animVars.set("headPosition", params.rigHeadPosition); - _animVars.set("headRotation", params.rigHeadOrientation); - if (params.hipsEnabled) { + if (headEnabled) { + _animVars.set("headPosition", headPose.trans()); + _animVars.set("headRotation", headPose.rot()); + if (hipsEnabled) { // Since there is an explicit hips ik target, switch the head to use the more flexible Spline IK chain type. // this will allow the spine to compress/expand and bend more natrually, ensuring that it can reach the head target position. _animVars.set("headType", (int)IKTarget::Type::Spline); @@ -1099,12 +1059,271 @@ void Rig::updateHeadAnimVars(const HeadParameters& params) { } } else { _animVars.unset("headPosition"); - _animVars.set("headRotation", params.rigHeadOrientation); + _animVars.set("headRotation", headPose.rot()); _animVars.set("headType", (int)IKTarget::Type::RotationOnly); } } } +void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnabled, float dt, + const AnimPose& leftHandPose, const AnimPose& rightHandPose, + float bodyCapsuleRadius, float bodyCapsuleHalfHeight, const glm::vec3& bodyCapsuleLocalOffset) { + + // Use this capsule to represent the avatar body. + int hipsIndex = indexOfJoint("Hips"); + glm::vec3 hipsTrans; + if (hipsIndex >= 0) { + hipsTrans = _internalPoseSet._absolutePoses[hipsIndex].trans(); + } + + const glm::vec3 bodyCapsuleCenter = hipsTrans - bodyCapsuleLocalOffset; + const glm::vec3 bodyCapsuleStart = bodyCapsuleCenter - glm::vec3(0, bodyCapsuleHalfHeight, 0); + const glm::vec3 bodyCapsuleEnd = bodyCapsuleCenter + glm::vec3(0, bodyCapsuleHalfHeight, 0); + + const float HAND_RADIUS = 0.05f; + + const float RELAX_DURATION = 0.6f; + const float CONTROL_DURATION = 0.4f; + const bool TO_CONTROLLED = true; + const bool FROM_CONTROLLED = false; + const bool LEFT_HAND = true; + const bool RIGHT_HAND = false; + + const float ELBOW_POLE_VECTOR_BLEND_FACTOR = 0.9f; + + if (leftHandEnabled) { + if (!_isLeftHandControlled) { + _leftHandControlTimeRemaining = CONTROL_DURATION; + _isLeftHandControlled = true; + } + + glm::vec3 handPosition = leftHandPose.trans(); + glm::quat handRotation = leftHandPose.rot(); + + if (_leftHandControlTimeRemaining > 0.0f) { + // Move hand from non-controlled position to controlled position. + _leftHandControlTimeRemaining = std::max(_leftHandControlTimeRemaining - dt, 0.0f); + AnimPose handPose(Vectors::ONE, handRotation, handPosition); + if (transitionHandPose(_leftHandControlTimeRemaining, CONTROL_DURATION, handPose, + LEFT_HAND, TO_CONTROLLED, handPose)) { + handPosition = handPose.trans(); + handRotation = handPose.rot(); + } + } + + if (!hipsEnabled) { + // prevent the hand IK targets from intersecting the body capsule + glm::vec3 displacement; + if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) { + handPosition -= displacement; + } + } + + _animVars.set("leftHandPosition", handPosition); + _animVars.set("leftHandRotation", handRotation); + _animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition); + + _lastLeftHandControlledPose = AnimPose(Vectors::ONE, handRotation, handPosition); + _isLeftHandControlled = true; + + // compute pole vector + int handJointIndex = _animSkeleton->nameToJointIndex("LeftHand"); + int armJointIndex = _animSkeleton->nameToJointIndex("LeftArm"); + int elbowJointIndex = _animSkeleton->nameToJointIndex("LeftForeArm"); + if (elbowJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0) { + glm::vec3 poleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, hipsIndex, true); + + // smooth toward desired pole vector from previous pole vector... to reduce jitter + if (!_prevLeftHandPoleVectorValid) { + _prevLeftHandPoleVectorValid = true; + _prevLeftHandPoleVector = poleVector; + } + glm::quat deltaRot = rotationBetween(_prevLeftHandPoleVector, poleVector); + glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, ELBOW_POLE_VECTOR_BLEND_FACTOR); + _prevLeftHandPoleVector = smoothDeltaRot * _prevLeftHandPoleVector; + + _animVars.set("leftHandPoleVectorEnabled", true); + _animVars.set("leftHandPoleReferenceVector", Vectors::UNIT_X); + _animVars.set("leftHandPoleVector", _prevLeftHandPoleVector); + } else { + _prevLeftHandPoleVectorValid = false; + _animVars.set("leftHandPoleVectorEnabled", false); + } + } else { + _prevLeftHandPoleVectorValid = false; + _animVars.set("leftHandPoleVectorEnabled", false); + + if (_isLeftHandControlled) { + _leftHandRelaxTimeRemaining = RELAX_DURATION; + _isLeftHandControlled = false; + } + + if (_leftHandRelaxTimeRemaining > 0.0f) { + // Move hand from controlled position to non-controlled position. + _leftHandRelaxTimeRemaining = std::max(_leftHandRelaxTimeRemaining - dt, 0.0f); + AnimPose handPose; + if (transitionHandPose(_leftHandRelaxTimeRemaining, RELAX_DURATION, _lastLeftHandControlledPose, + LEFT_HAND, FROM_CONTROLLED, handPose)) { + _animVars.set("leftHandPosition", handPose.trans()); + _animVars.set("leftHandRotation", handPose.rot()); + _animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition); + } + } else { + _animVars.unset("leftHandPosition"); + _animVars.unset("leftHandRotation"); + _animVars.set("leftHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition); + } + } + + if (rightHandEnabled) { + if (!_isRightHandControlled) { + _rightHandControlTimeRemaining = CONTROL_DURATION; + _isRightHandControlled = true; + } + + glm::vec3 handPosition = rightHandPose.trans(); + glm::quat handRotation = rightHandPose.rot(); + + if (_rightHandControlTimeRemaining > 0.0f) { + // Move hand from non-controlled position to controlled position. + _rightHandControlTimeRemaining = std::max(_rightHandControlTimeRemaining - dt, 0.0f); + AnimPose handPose(Vectors::ONE, handRotation, handPosition); + if (transitionHandPose(_rightHandControlTimeRemaining, CONTROL_DURATION, handPose, RIGHT_HAND, TO_CONTROLLED, handPose)) { + handPosition = handPose.trans(); + handRotation = handPose.rot(); + } + } + + if (!hipsEnabled) { + // prevent the hand IK targets from intersecting the body capsule + glm::vec3 displacement; + if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) { + handPosition -= displacement; + } + } + + _animVars.set("rightHandPosition", handPosition); + _animVars.set("rightHandRotation", handRotation); + _animVars.set("rightHandType", (int)IKTarget::Type::RotationAndPosition); + + _lastRightHandControlledPose = AnimPose(Vectors::ONE, handRotation, handPosition); + _isRightHandControlled = true; + + // compute pole vector + int handJointIndex = _animSkeleton->nameToJointIndex("RightHand"); + int armJointIndex = _animSkeleton->nameToJointIndex("RightArm"); + int elbowJointIndex = _animSkeleton->nameToJointIndex("RightForeArm"); + if (elbowJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0) { + glm::vec3 poleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, hipsIndex, false); + + // smooth toward desired pole vector from previous pole vector... to reduce jitter + if (!_prevRightHandPoleVectorValid) { + _prevRightHandPoleVectorValid = true; + _prevRightHandPoleVector = poleVector; + } + glm::quat deltaRot = rotationBetween(_prevRightHandPoleVector, poleVector); + glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, ELBOW_POLE_VECTOR_BLEND_FACTOR); + _prevRightHandPoleVector = smoothDeltaRot * _prevRightHandPoleVector; + + _animVars.set("rightHandPoleVectorEnabled", true); + _animVars.set("rightHandPoleReferenceVector", -Vectors::UNIT_X); + _animVars.set("rightHandPoleVector", _prevRightHandPoleVector); + } else { + _prevRightHandPoleVectorValid = false; + _animVars.set("rightHandPoleVectorEnabled", false); + } + } else { + _prevRightHandPoleVectorValid = false; + _animVars.set("rightHandPoleVectorEnabled", false); + + if (_isRightHandControlled) { + _rightHandRelaxTimeRemaining = RELAX_DURATION; + _isRightHandControlled = false; + } + + if (_rightHandRelaxTimeRemaining > 0.0f) { + // Move hand from controlled position to non-controlled position. + _rightHandRelaxTimeRemaining = std::max(_rightHandRelaxTimeRemaining - dt, 0.0f); + AnimPose handPose; + if (transitionHandPose(_rightHandRelaxTimeRemaining, RELAX_DURATION, _lastRightHandControlledPose, RIGHT_HAND, FROM_CONTROLLED, handPose)) { + _animVars.set("rightHandPosition", handPose.trans()); + _animVars.set("rightHandRotation", handPose.rot()); + _animVars.set("rightHandType", (int)IKTarget::Type::RotationAndPosition); + } + } else { + _animVars.unset("rightHandPosition"); + _animVars.unset("rightHandRotation"); + _animVars.set("rightHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition); + } + } +} + +void Rig::updateFeet(bool leftFootEnabled, bool rightFootEnabled, const AnimPose& leftFootPose, const AnimPose& rightFootPose) { + + const float KNEE_POLE_VECTOR_BLEND_FACTOR = 0.9f; + + int hipsIndex = indexOfJoint("Hips"); + + if (leftFootEnabled) { + glm::vec3 footPosition = leftFootPose.trans(); + glm::quat footRotation = leftFootPose.rot(); + _animVars.set("leftFootPosition", footPosition); + _animVars.set("leftFootRotation", footRotation); + _animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition); + + int footJointIndex = _animSkeleton->nameToJointIndex("LeftFoot"); + glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, footRotation, hipsIndex); + + // smooth toward desired pole vector from previous pole vector... to reduce jitter + if (!_prevLeftFootPoleVectorValid) { + _prevLeftFootPoleVectorValid = true; + _prevLeftFootPoleVector = poleVector; + } + glm::quat deltaRot = rotationBetween(_prevLeftFootPoleVector, poleVector); + glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, KNEE_POLE_VECTOR_BLEND_FACTOR); + _prevLeftFootPoleVector = smoothDeltaRot * _prevLeftFootPoleVector; + + _animVars.set("leftFootPoleVectorEnabled", true); + _animVars.set("leftFootPoleReferenceVector", Vectors::UNIT_Z); + _animVars.set("leftFootPoleVector", _prevLeftFootPoleVector); + } else { + _animVars.unset("leftFootPosition"); + _animVars.unset("leftFootRotation"); + _animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition); + _animVars.set("leftFootPoleVectorEnabled", false); + _prevLeftFootPoleVectorValid = false; + } + + if (rightFootEnabled) { + glm::vec3 footPosition = rightFootPose.trans(); + glm::quat footRotation = rightFootPose.rot(); + _animVars.set("rightFootPosition", footPosition); + _animVars.set("rightFootRotation", footRotation); + _animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition); + + int footJointIndex = _animSkeleton->nameToJointIndex("RightFoot"); + glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, footRotation, hipsIndex); + + // smooth toward desired pole vector from previous pole vector... to reduce jitter + if (!_prevRightFootPoleVectorValid) { + _prevRightFootPoleVectorValid = true; + _prevRightFootPoleVector = poleVector; + } + glm::quat deltaRot = rotationBetween(_prevRightFootPoleVector, poleVector); + glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, KNEE_POLE_VECTOR_BLEND_FACTOR); + _prevRightFootPoleVector = smoothDeltaRot * _prevRightFootPoleVector; + + _animVars.set("rightFootPoleVectorEnabled", true); + _animVars.set("rightFootPoleReferenceVector", Vectors::UNIT_Z); + _animVars.set("rightFootPoleVector", _prevRightFootPoleVector); + } else { + _animVars.unset("rightFootPosition"); + _animVars.unset("rightFootRotation"); + _animVars.set("rightFootPoleVectorEnabled", false); + _animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition); + } +} + void Rig::updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm::quat& modelRotation, const glm::vec3& lookAtSpot, const glm::vec3& saccade) { // TODO: does not properly handle avatar scale. @@ -1196,261 +1415,65 @@ glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, const glm::quat& foot return glm::normalize(lerp(footForward, hipsForward, FOOT_TO_HIPS_BLEND_FACTOR)); } -void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, float dt) { - if (_animSkeleton && _animNode) { - const float HAND_RADIUS = 0.05f; - int hipsIndex = indexOfJoint("Hips"); - glm::vec3 hipsTrans; - if (hipsIndex >= 0) { - hipsTrans = _internalPoseSet._absolutePoses[hipsIndex].trans(); - } +void Rig::updateFromControllerParameters(const ControllerParameters& params, float dt) { + if (!_animSkeleton || !_animNode) { + return; + } - // Use this capsule to represent the avatar body. - const float bodyCapsuleRadius = params.bodyCapsuleRadius; - const glm::vec3 bodyCapsuleCenter = hipsTrans - params.bodyCapsuleLocalOffset; - const glm::vec3 bodyCapsuleStart = bodyCapsuleCenter - glm::vec3(0, params.bodyCapsuleHalfHeight, 0); - const glm::vec3 bodyCapsuleEnd = bodyCapsuleCenter + glm::vec3(0, params.bodyCapsuleHalfHeight, 0); + _animVars.set("isTalking", params.isTalking); + _animVars.set("notIsTalking", !params.isTalking); - // TODO: add isHipsEnabled - bool bodySensorTrackingEnabled = params.isLeftFootEnabled || params.isRightFootEnabled; + bool headEnabled = params.controllerActiveFlags[ControllerType_Head]; + bool leftHandEnabled = params.controllerActiveFlags[ControllerType_LeftHand]; + bool rightHandEnabled = params.controllerActiveFlags[ControllerType_RightHand]; + bool hipsEnabled = params.controllerActiveFlags[ControllerType_Hips]; + bool leftFootEnabled = params.controllerActiveFlags[ControllerType_LeftFoot]; + bool rightFootEnabled = params.controllerActiveFlags[ControllerType_RightFoot]; + bool leftArmEnabled = params.controllerActiveFlags[ControllerType_LeftArm]; + bool rightArmEnabled = params.controllerActiveFlags[ControllerType_RightArm]; + bool spine2Enabled = params.controllerActiveFlags[ControllerType_Spine2]; - const float RELAX_DURATION = 0.6f; - const float CONTROL_DURATION = 0.4f; - const bool TO_CONTROLLED = true; - const bool FROM_CONTROLLED = false; - const bool LEFT_HAND = true; - const bool RIGHT_HAND = false; + updateHead(headEnabled, hipsEnabled, params.controllerPoses[ControllerType_Head]); - const float KNEE_POLE_VECTOR_BLEND_FACTOR = 0.9f; - const float ELBOW_POLE_VECTOR_BLEND_FACTOR = 0.9f; + updateHands(leftHandEnabled, rightHandEnabled, hipsEnabled, dt, + params.controllerPoses[ControllerType_LeftHand], params.controllerPoses[ControllerType_RightHand], + params.bodyCapsuleRadius, params.bodyCapsuleHalfHeight, params.bodyCapsuleLocalOffset); - if (params.isLeftEnabled) { - if (!_isLeftHandControlled) { - _leftHandControlTimeRemaining = CONTROL_DURATION; - _isLeftHandControlled = true; - } + updateFeet(leftFootEnabled, rightFootEnabled, + params.controllerPoses[ControllerType_LeftFoot], params.controllerPoses[ControllerType_RightFoot]); - glm::vec3 handPosition = params.leftPosition; - glm::quat handRotation = params.leftOrientation; + if (hipsEnabled) { + _animVars.set("solutionSource", (int)AnimInverseKinematics::SolutionSource::RelaxToLimitCenterPoses); + _animVars.set("hipsType", (int)IKTarget::Type::RotationAndPosition); + _animVars.set("hipsPosition", params.controllerPoses[ControllerType_Hips].trans()); + _animVars.set("hipsRotation", params.controllerPoses[ControllerType_Hips].rot()); + } else { + _animVars.set("solutionSource", (int)AnimInverseKinematics::SolutionSource::RelaxToUnderPoses); + _animVars.set("hipsType", (int)IKTarget::Type::Unknown); + } - if (_leftHandControlTimeRemaining > 0.0f) { - // Move hand from non-controlled position to controlled position. - _leftHandControlTimeRemaining = std::max(_leftHandControlTimeRemaining - dt, 0.0f); - AnimPose handPose(Vectors::ONE, handRotation, handPosition); - if (transitionHandPose(_leftHandControlTimeRemaining, CONTROL_DURATION, handPose, - LEFT_HAND, TO_CONTROLLED, handPose)) { - handPosition = handPose.trans(); - handRotation = handPose.rot(); - } - } + if (hipsEnabled && spine2Enabled) { + _animVars.set("spine2Type", (int)IKTarget::Type::Spline); + _animVars.set("spine2Position", params.controllerPoses[ControllerType_Spine2].trans()); + _animVars.set("spine2Rotation", params.controllerPoses[ControllerType_Spine2].rot()); + } else { + _animVars.set("spine2Type", (int)IKTarget::Type::Unknown); + } - if (!bodySensorTrackingEnabled) { - // prevent the hand IK targets from intersecting the body capsule - glm::vec3 displacement; - if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) { - handPosition -= displacement; - } - } + if (leftArmEnabled) { + _animVars.set("leftArmType", (int)IKTarget::Type::RotationAndPosition); + _animVars.set("leftArmPosition", params.controllerPoses[ControllerType_LeftArm].trans()); + _animVars.set("leftArmRotation", params.controllerPoses[ControllerType_LeftArm].rot()); + } else { + _animVars.set("leftArmType", (int)IKTarget::Type::Unknown); + } - _animVars.set("leftHandPosition", handPosition); - _animVars.set("leftHandRotation", handRotation); - _animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition); - - _lastLeftHandControlledPose = AnimPose(Vectors::ONE, handRotation, handPosition); - - _isLeftHandControlled = true; - _lastLeftHandControlledPose = AnimPose(glm::vec3(1.0f), params.leftOrientation, handPosition); - - // compute pole vector - int handJointIndex = _animSkeleton->nameToJointIndex("LeftHand"); - int armJointIndex = _animSkeleton->nameToJointIndex("LeftArm"); - int elbowJointIndex = _animSkeleton->nameToJointIndex("LeftForeArm"); - if (elbowJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0) { - glm::vec3 poleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, hipsIndex, true); - - // smooth toward desired pole vector from previous pole vector... to reduce jitter - if (!_prevLeftHandPoleVectorValid) { - _prevLeftHandPoleVectorValid = true; - _prevLeftHandPoleVector = poleVector; - } - glm::quat deltaRot = rotationBetween(_prevLeftHandPoleVector, poleVector); - glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, ELBOW_POLE_VECTOR_BLEND_FACTOR); - _prevLeftHandPoleVector = smoothDeltaRot * _prevLeftHandPoleVector; - - _animVars.set("leftHandPoleVectorEnabled", true); - _animVars.set("leftHandPoleReferenceVector", Vectors::UNIT_X); - _animVars.set("leftHandPoleVector", _prevLeftHandPoleVector); - } else { - _prevLeftHandPoleVectorValid = false; - _animVars.set("leftHandPoleVectorEnabled", false); - } - } else { - _prevLeftHandPoleVectorValid = false; - _animVars.set("leftHandPoleVectorEnabled", false); - - if (_isLeftHandControlled) { - _leftHandRelaxTimeRemaining = RELAX_DURATION; - _isLeftHandControlled = false; - } - - if (_leftHandRelaxTimeRemaining > 0.0f) { - // Move hand from controlled position to non-controlled position. - _leftHandRelaxTimeRemaining = std::max(_leftHandRelaxTimeRemaining - dt, 0.0f); - AnimPose handPose; - if (transitionHandPose(_leftHandRelaxTimeRemaining, RELAX_DURATION, _lastLeftHandControlledPose, - LEFT_HAND, FROM_CONTROLLED, handPose)) { - _animVars.set("leftHandPosition", handPose.trans()); - _animVars.set("leftHandRotation", handPose.rot()); - _animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition); - } - } else { - _animVars.unset("leftHandPosition"); - _animVars.unset("leftHandRotation"); - _animVars.set("leftHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition); - } - } - - if (params.isRightEnabled) { - if (!_isRightHandControlled) { - _rightHandControlTimeRemaining = CONTROL_DURATION; - _isRightHandControlled = true; - } - - glm::vec3 handPosition = params.rightPosition; - glm::quat handRotation = params.rightOrientation; - - if (_rightHandControlTimeRemaining > 0.0f) { - // Move hand from non-controlled position to controlled position. - _rightHandControlTimeRemaining = std::max(_rightHandControlTimeRemaining - dt, 0.0f); - AnimPose handPose(Vectors::ONE, handRotation, handPosition); - if (transitionHandPose(_rightHandControlTimeRemaining, CONTROL_DURATION, handPose, RIGHT_HAND, TO_CONTROLLED, - handPose)) { - handPosition = handPose.trans(); - handRotation = handPose.rot(); - } - } - - if (!bodySensorTrackingEnabled) { - // prevent the hand IK targets from intersecting the body capsule - glm::vec3 displacement; - if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) { - handPosition -= displacement; - } - } - - _animVars.set("rightHandPosition", handPosition); - _animVars.set("rightHandRotation", handRotation); - _animVars.set("rightHandType", (int)IKTarget::Type::RotationAndPosition); - - _lastRightHandControlledPose = AnimPose(Vectors::ONE, handRotation, handPosition); - - _isRightHandControlled = true; - _lastRightHandControlledPose = AnimPose(glm::vec3(1.0f), params.rightOrientation, handPosition); - - // compute pole vector - int handJointIndex = _animSkeleton->nameToJointIndex("RightHand"); - int armJointIndex = _animSkeleton->nameToJointIndex("RightArm"); - int elbowJointIndex = _animSkeleton->nameToJointIndex("RightForeArm"); - if (elbowJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0) { - glm::vec3 poleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, hipsIndex, false); - - // smooth toward desired pole vector from previous pole vector... to reduce jitter - if (!_prevRightHandPoleVectorValid) { - _prevRightHandPoleVectorValid = true; - _prevRightHandPoleVector = poleVector; - } - glm::quat deltaRot = rotationBetween(_prevRightHandPoleVector, poleVector); - glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, ELBOW_POLE_VECTOR_BLEND_FACTOR); - _prevRightHandPoleVector = smoothDeltaRot * _prevRightHandPoleVector; - - _animVars.set("rightHandPoleVectorEnabled", true); - _animVars.set("rightHandPoleReferenceVector", -Vectors::UNIT_X); - _animVars.set("rightHandPoleVector", _prevRightHandPoleVector); - } else { - _prevRightHandPoleVectorValid = false; - _animVars.set("rightHandPoleVectorEnabled", false); - } - - } else { - _prevRightHandPoleVectorValid = false; - _animVars.set("rightHandPoleVectorEnabled", false); - - if (_isRightHandControlled) { - _rightHandRelaxTimeRemaining = RELAX_DURATION; - _isRightHandControlled = false; - } - - if (_rightHandRelaxTimeRemaining > 0.0f) { - // Move hand from controlled position to non-controlled position. - _rightHandRelaxTimeRemaining = std::max(_rightHandRelaxTimeRemaining - dt, 0.0f); - AnimPose handPose; - if (transitionHandPose(_rightHandRelaxTimeRemaining, RELAX_DURATION, _lastRightHandControlledPose, RIGHT_HAND, - FROM_CONTROLLED, handPose)) { - _animVars.set("rightHandPosition", handPose.trans()); - _animVars.set("rightHandRotation", handPose.rot()); - _animVars.set("rightHandType", (int)IKTarget::Type::RotationAndPosition); - } - } else { - _animVars.unset("rightHandPosition"); - _animVars.unset("rightHandRotation"); - _animVars.set("rightHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition); - } - } - - if (params.isLeftFootEnabled) { - _animVars.set("leftFootPosition", params.leftFootPosition); - _animVars.set("leftFootRotation", params.leftFootOrientation); - _animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition); - - int footJointIndex = _animSkeleton->nameToJointIndex("LeftFoot"); - glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, params.leftFootOrientation, hipsIndex); - - // smooth toward desired pole vector from previous pole vector... to reduce jitter - if (!_prevLeftFootPoleVectorValid) { - _prevLeftFootPoleVectorValid = true; - _prevLeftFootPoleVector = poleVector; - } - glm::quat deltaRot = rotationBetween(_prevLeftFootPoleVector, poleVector); - glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, KNEE_POLE_VECTOR_BLEND_FACTOR); - _prevLeftFootPoleVector = smoothDeltaRot * _prevLeftFootPoleVector; - - _animVars.set("leftFootPoleVectorEnabled", true); - _animVars.set("leftFootPoleReferenceVector", Vectors::UNIT_Z); - _animVars.set("leftFootPoleVector", _prevLeftFootPoleVector); - } else { - _animVars.unset("leftFootPosition"); - _animVars.unset("leftFootRotation"); - _animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition); - _animVars.set("leftFootPoleVectorEnabled", false); - _prevLeftFootPoleVectorValid = false; - } - - if (params.isRightFootEnabled) { - _animVars.set("rightFootPosition", params.rightFootPosition); - _animVars.set("rightFootRotation", params.rightFootOrientation); - _animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition); - - int footJointIndex = _animSkeleton->nameToJointIndex("RightFoot"); - glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, params.rightFootOrientation, hipsIndex); - - // smooth toward desired pole vector from previous pole vector... to reduce jitter - if (!_prevRightFootPoleVectorValid) { - _prevRightFootPoleVectorValid = true; - _prevRightFootPoleVector = poleVector; - } - glm::quat deltaRot = rotationBetween(_prevRightFootPoleVector, poleVector); - glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, KNEE_POLE_VECTOR_BLEND_FACTOR); - _prevRightFootPoleVector = smoothDeltaRot * _prevRightFootPoleVector; - - _animVars.set("rightFootPoleVectorEnabled", true); - _animVars.set("rightFootPoleReferenceVector", Vectors::UNIT_Z); - _animVars.set("rightFootPoleVector", _prevRightFootPoleVector); - } else { - _animVars.unset("rightFootPosition"); - _animVars.unset("rightFootRotation"); - _animVars.set("rightFootPoleVectorEnabled", false); - _animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition); - } + if (rightArmEnabled) { + _animVars.set("rightArmType", (int)IKTarget::Type::RotationAndPosition); + _animVars.set("rightArmPosition", params.controllerPoses[ControllerType_RightArm].trans()); + _animVars.set("rightArmRotation", params.controllerPoses[ControllerType_RightArm].rot()); + } else { + _animVars.set("rightArmType", (int)IKTarget::Type::Unknown); } } diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index d101425a77..d876b8c22c 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -41,21 +41,26 @@ public: bool useNames; }; - struct HeadParameters { - glm::mat4 hipsMatrix = glm::mat4(); // rig space - glm::mat4 spine2Matrix = glm::mat4(); // rig space - glm::quat rigHeadOrientation = glm::quat(); // rig space (-z forward) - glm::vec3 rigHeadPosition = glm::vec3(); // rig space - glm::vec3 rightArmPosition = glm::vec3(); // rig space - glm::quat rightArmRotation = glm::quat(); // rig space - glm::vec3 leftArmPosition = glm::vec3(); // rig space - glm::quat leftArmRotation = glm::quat(); // rig space - bool hipsEnabled = false; - bool headEnabled = false; - bool spine2Enabled = false; - bool leftArmEnabled = false; - bool rightArmEnabled = false; - bool isTalking = false; + enum ControllerType { + ControllerType_Head = 0, + ControllerType_LeftHand, + ControllerType_RightHand, + ControllerType_Hips, + ControllerType_LeftFoot, + ControllerType_RightFoot, + ControllerType_LeftArm, + ControllerType_RightArm, + ControllerType_Spine2, + NumControllerTypes + }; + + struct ControllerParameters { + AnimPose controllerPoses[NumControllerTypes]; // rig space + bool controllerActiveFlags[NumControllerTypes]; + bool isTalking; + float bodyCapsuleRadius; + float bodyCapsuleHalfHeight; + glm::vec3 bodyCapsuleLocalOffset; }; struct EyeParameters { @@ -67,25 +72,6 @@ public: int rightEyeJointIndex = -1; }; - struct HandAndFeetParameters { - bool isLeftEnabled; - bool isRightEnabled; - float bodyCapsuleRadius; - float bodyCapsuleHalfHeight; - glm::vec3 bodyCapsuleLocalOffset; - glm::vec3 leftPosition = glm::vec3(); // rig space - glm::quat leftOrientation = glm::quat(); // rig space (z forward) - glm::vec3 rightPosition = glm::vec3(); // rig space - glm::quat rightOrientation = glm::quat(); // rig space (z forward) - - bool isLeftFootEnabled; - bool isRightFootEnabled; - glm::vec3 leftFootPosition = glm::vec3(); // rig space - glm::quat leftFootOrientation = glm::quat(); // rig space (z forward) - glm::vec3 rightFootPosition = glm::vec3(); // rig space - glm::quat rightFootOrientation = glm::quat(); // rig space (z forward) - }; - enum class CharacterControllerState { Ground = 0, Takeoff, @@ -192,9 +178,8 @@ public: // legacy void clearJointStatePriorities(); - void updateFromHeadParameters(const HeadParameters& params, float dt); + void updateFromControllerParameters(const ControllerParameters& params, float dt); void updateFromEyeParameters(const EyeParameters& params); - void updateFromHandAndFeetParameters(const HandAndFeetParameters& params, float dt); void initAnimGraph(const QUrl& url); @@ -244,7 +229,11 @@ protected: void applyOverridePoses(); void buildAbsoluteRigPoses(const AnimPoseVec& relativePoses, AnimPoseVec& absolutePosesOut); - void updateHeadAnimVars(const HeadParameters& params); + void updateHead(bool headEnabled, bool hipsEnabled, const AnimPose& headMatrix); + void updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnabled, float dt, + const AnimPose& leftHandPose, const AnimPose& rightHandPose, + float bodyCapsuleRadius, float bodyCapsuleHalfHeight, const glm::vec3& bodyCapsuleLocalOffset); + void updateFeet(bool leftFootEnabled, bool rightFootEnabled, const AnimPose& leftFootPose, const AnimPose& rightFootPose); void updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm::quat& modelRotation, const glm::vec3& lookAt, const glm::vec3& saccade); void calcAnimAlpha(float speed, const std::vector& referenceSpeeds, float* alphaOut) const; From 1979ed7f3a08557d8fae5942b55d7d109befd057 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 19 Jun 2017 16:58:11 -0700 Subject: [PATCH 16/42] Disable elbow pole vector if arm/shoulder pucks are enabled --- libraries/animation/src/Rig.cpp | 8 ++++---- libraries/animation/src/Rig.h | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 0ee42372b1..ef5bd3c032 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1065,7 +1065,7 @@ void Rig::updateHead(bool headEnabled, bool hipsEnabled, const AnimPose& headPos } } -void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnabled, float dt, +void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnabled, bool leftArmEnabled, bool rightArmEnabled, float dt, const AnimPose& leftHandPose, const AnimPose& rightHandPose, float bodyCapsuleRadius, float bodyCapsuleHalfHeight, const glm::vec3& bodyCapsuleLocalOffset) { @@ -1130,7 +1130,7 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab int handJointIndex = _animSkeleton->nameToJointIndex("LeftHand"); int armJointIndex = _animSkeleton->nameToJointIndex("LeftArm"); int elbowJointIndex = _animSkeleton->nameToJointIndex("LeftForeArm"); - if (elbowJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0) { + if (!leftArmEnabled && elbowJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0) { glm::vec3 poleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, hipsIndex, true); // smooth toward desired pole vector from previous pole vector... to reduce jitter @@ -1213,7 +1213,7 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab int handJointIndex = _animSkeleton->nameToJointIndex("RightHand"); int armJointIndex = _animSkeleton->nameToJointIndex("RightArm"); int elbowJointIndex = _animSkeleton->nameToJointIndex("RightForeArm"); - if (elbowJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0) { + if (!rightArmEnabled && elbowJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0) { glm::vec3 poleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, hipsIndex, false); // smooth toward desired pole vector from previous pole vector... to reduce jitter @@ -1435,7 +1435,7 @@ void Rig::updateFromControllerParameters(const ControllerParameters& params, flo updateHead(headEnabled, hipsEnabled, params.controllerPoses[ControllerType_Head]); - updateHands(leftHandEnabled, rightHandEnabled, hipsEnabled, dt, + updateHands(leftHandEnabled, rightHandEnabled, hipsEnabled, leftArmEnabled, rightArmEnabled, dt, params.controllerPoses[ControllerType_LeftHand], params.controllerPoses[ControllerType_RightHand], params.bodyCapsuleRadius, params.bodyCapsuleHalfHeight, params.bodyCapsuleLocalOffset); diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index d876b8c22c..413b598079 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -230,7 +230,7 @@ protected: void buildAbsoluteRigPoses(const AnimPoseVec& relativePoses, AnimPoseVec& absolutePosesOut); void updateHead(bool headEnabled, bool hipsEnabled, const AnimPose& headMatrix); - void updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnabled, float dt, + void updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnabled, bool leftArmEnabled, bool rightArmEnabled, float dt, const AnimPose& leftHandPose, const AnimPose& rightHandPose, float bodyCapsuleRadius, float bodyCapsuleHalfHeight, const glm::vec3& bodyCapsuleLocalOffset); void updateFeet(bool leftFootEnabled, bool rightFootEnabled, const AnimPose& leftFootPose, const AnimPose& rightFootPose); From cd1beceb7582583f4c8f5cece2e5948be21a999e Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 20 Jun 2017 17:02:50 -0700 Subject: [PATCH 17/42] Added script that will attach an entity to a vive sensor. --- scripts/developer/tests/puck-attach.js | 131 +++++++++++++++++++++++++ 1 file changed, 131 insertions(+) create mode 100644 scripts/developer/tests/puck-attach.js diff --git a/scripts/developer/tests/puck-attach.js b/scripts/developer/tests/puck-attach.js new file mode 100644 index 0000000000..ff86d31809 --- /dev/null +++ b/scripts/developer/tests/puck-attach.js @@ -0,0 +1,131 @@ +// +// debugatar.js +// +/* eslint indent: ["error", 4, { "outerIIFEBody": 0 }] */ +/* global Xform */ +Script.include("/~/system/libraries/Xform.js"); + +(function() { // BEGIN LOCAL_SCOPE + +var TABLET_BUTTON_NAME = "PUCKATTACH"; +var HTML_URL = "https://s3.amazonaws.com/hifi-public/tony/html/puck-attach.html"; + +var tablet = Tablet.getTablet("com.highfidelity.interface.tablet.system"); +var tabletButton = tablet.addButton({ + text: TABLET_BUTTON_NAME, + icon: "https://s3.amazonaws.com/hifi-public/tony/icons/tpose-i.svg", + activeIcon: "https://s3.amazonaws.com/hifi-public/tony/icons/tpose-a.svg" +}); + +tabletButton.clicked.connect(function () { + if (shown) { + tablet.gotoHomeScreen(); + } else { + tablet.gotoWebScreen(HTML_URL); + } +}); + +var shown = false; +var attachedEntity; +var attachedObj; + +function onScreenChanged(type, url) { + if (type === "Web" && url === HTML_URL) { + tabletButton.editProperties({isActive: true}); + if (!shown) { + // hook up to event bridge + tablet.webEventReceived.connect(onWebEventReceived); + } + shown = true; + } else { + tabletButton.editProperties({isActive: false}); + if (shown) { + // disconnect from event bridge + tablet.webEventReceived.disconnect(onWebEventReceived); + } + shown = false; + } +} + +tablet.screenChanged.connect(onScreenChanged); + +function attach(obj) { + attachedEntity = Entities.addEntity({ + type: "Model", + name: "puck-attach-entity", + modelURL: obj.modelurl + }); + attachedObj = obj; + var localPos = {x: Number(obj.posx), y: Number(obj.posy), z: Number(obj.posz)}; + var localRot = Quat.fromVec3Degrees({x: Number(obj.rotx), y: Number(obj.roty), z: Number(obj.rotz)}); + attachedObj.localXform = new Xform(localRot, localPos); + var key = "TrackedObject" + pad(attachedObj.puckno, 2); + attachedObj.key = key; + + print("AJT: attachedObj = " + JSON.stringify(attachedObj)); + + Script.update.connect(update); + update(0.001); +} + +function remove() { + if (attachedEntity) { + Script.update.disconnect(update); + Entities.deleteEntity(attachedEntity); + attachedEntity = undefined; + } + attachedObj = undefined; +} + +function pad(num, size) { + var s = "000000000" + num; + return s.substr(s.length-size); +} + +function update(dt) { + if (attachedEntity && attachedObj && Controller.Hardware.Vive) { + var pose = Controller.getPoseValue(Controller.Hardware.Vive[attachedObj.key]); + var avatarXform = new Xform(MyAvatar.orientation, MyAvatar.position); + var puckXform = new Xform(pose.rotation, pose.translation); + var finalXform = Xform.mul(avatarXform, Xform.mul(puckXform, attachedObj.localXform)); + if (pose && pose.valid) { + Entities.editEntity(attachedEntity, { + position: finalXform.pos, + rotation: finalXform.rot + }); + } else { + if (pose) { + print("AJT: WARNING: invalid pose for " + attachedObj.key); + } else { + print("AJT: WARNING: could not find key " + attachedObj.key); + } + } + } +} + +function onWebEventReceived(msg) { + var obj = {}; + try { + obj = JSON.parse(msg); + } catch (err) { + return; + } + if (obj.cmd === "attach") { + remove(); + attach(obj); + } else if (obj.cmd === "detach") { + remove(); + } +} + +Script.scriptEnding.connect(function () { + remove(); + tablet.removeButton(tabletButton); + if (shown) { + tablet.webEventReceived.disconnect(onWebEventReceived); + tablet.gotoHomeScreen(); + } + tablet.screenChanged.disconnect(onScreenChanged); +}); + +}()); // END LOCAL_SCOPE From 287dc3e1b83bd68ba6b85d09d5fc084d84a43c2b Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 20 Jun 2017 17:10:53 -0700 Subject: [PATCH 18/42] updated license and button name --- scripts/developer/tests/puck-attach.js | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/scripts/developer/tests/puck-attach.js b/scripts/developer/tests/puck-attach.js index ff86d31809..7ed38eb5d0 100644 --- a/scripts/developer/tests/puck-attach.js +++ b/scripts/developer/tests/puck-attach.js @@ -1,13 +1,18 @@ // -// debugatar.js +// Created by Anthony J. Thibault on 2017/06/20 +// Copyright 2017 High Fidelity, Inc. // +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + /* eslint indent: ["error", 4, { "outerIIFEBody": 0 }] */ /* global Xform */ Script.include("/~/system/libraries/Xform.js"); (function() { // BEGIN LOCAL_SCOPE -var TABLET_BUTTON_NAME = "PUCKATTACH"; +var TABLET_BUTTON_NAME = "PUCKTACH"; var HTML_URL = "https://s3.amazonaws.com/hifi-public/tony/html/puck-attach.html"; var tablet = Tablet.getTablet("com.highfidelity.interface.tablet.system"); From e84d0358cc9d55aef5f35a161be68698b8dd9351 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 20 Jun 2017 17:18:35 -0700 Subject: [PATCH 19/42] updated images --- scripts/developer/tests/puck-attach.js | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/scripts/developer/tests/puck-attach.js b/scripts/developer/tests/puck-attach.js index 7ed38eb5d0..69d6c9e39b 100644 --- a/scripts/developer/tests/puck-attach.js +++ b/scripts/developer/tests/puck-attach.js @@ -18,8 +18,8 @@ var HTML_URL = "https://s3.amazonaws.com/hifi-public/tony/html/puck-attach.html" var tablet = Tablet.getTablet("com.highfidelity.interface.tablet.system"); var tabletButton = tablet.addButton({ text: TABLET_BUTTON_NAME, - icon: "https://s3.amazonaws.com/hifi-public/tony/icons/tpose-i.svg", - activeIcon: "https://s3.amazonaws.com/hifi-public/tony/icons/tpose-a.svg" + icon: "https://s3.amazonaws.com/hifi-public/tony/icons/puck-i.svg", + activeIcon: "https://s3.amazonaws.com/hifi-public/tony/icons/puck-a.svg" }); tabletButton.clicked.connect(function () { From b78e2781020c44737c3e406de83b5675361433e9 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 20 Jun 2017 17:19:57 -0700 Subject: [PATCH 20/42] adjusted print statements --- scripts/developer/tests/puck-attach.js | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/scripts/developer/tests/puck-attach.js b/scripts/developer/tests/puck-attach.js index 69d6c9e39b..807977794d 100644 --- a/scripts/developer/tests/puck-attach.js +++ b/scripts/developer/tests/puck-attach.js @@ -67,7 +67,7 @@ function attach(obj) { var key = "TrackedObject" + pad(attachedObj.puckno, 2); attachedObj.key = key; - print("AJT: attachedObj = " + JSON.stringify(attachedObj)); + print("PUCK-ATTACH: attachedObj = " + JSON.stringify(attachedObj)); Script.update.connect(update); update(0.001); @@ -100,9 +100,9 @@ function update(dt) { }); } else { if (pose) { - print("AJT: WARNING: invalid pose for " + attachedObj.key); + print("PUCK-ATTACH: WARNING: invalid pose for " + attachedObj.key); } else { - print("AJT: WARNING: could not find key " + attachedObj.key); + print("PUCK-ATTACH: WARNING: could not find key " + attachedObj.key); } } } From 0f51236fb06103773c8a0aac3210f95b9ad17506 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Wed, 21 Jun 2017 14:36:42 -0700 Subject: [PATCH 21/42] Rig.cpp: take avatar scale into account when computing elbow pole vector --- libraries/animation/src/Rig.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index ef5bd3c032..51480e8ef1 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1383,8 +1383,10 @@ glm::vec3 Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIn glm::vec3 y = hipsPose.rot() * Vectors::UNIT_Y; // project d onto n. glm::vec3 dProj = d - glm::dot(d, n) * n; - const float LATERAL_OFFSET = 0.333f; - const float VERTICAL_OFFSET = -0.333f; + + float avatarScale = extractUniformScale(_modelOffset); + const float LATERAL_OFFSET = 0.333f * avatarScale; + const float VERTICAL_OFFSET = -0.333f * avatarScale; // give dProj a bit of offset away from the body. glm::vec3 dProjWithOffset = dProj + sign * LATERAL_OFFSET * n + y * VERTICAL_OFFSET; From 7ff576ef1897b97ba4a05f3c804e6f060d108637 Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Thu, 22 Jun 2017 10:07:39 -0700 Subject: [PATCH 22/42] add memory-debugging macro to some cmake files --- assignment-client/CMakeLists.txt | 2 ++ domain-server/CMakeLists.txt | 2 ++ ice-server/CMakeLists.txt | 2 ++ interface/CMakeLists.txt | 2 ++ libraries/gl/CMakeLists.txt | 1 + tests/controllers/CMakeLists.txt | 4 +++- tests/qt59/CMakeLists.txt | 4 +++- tests/recording/CMakeLists.txt | 1 + tools/vhacd-util/CMakeLists.txt | 2 ++ 9 files changed, 18 insertions(+), 2 deletions(-) diff --git a/assignment-client/CMakeLists.txt b/assignment-client/CMakeLists.txt index 54afabfd21..6b3101f4e3 100644 --- a/assignment-client/CMakeLists.txt +++ b/assignment-client/CMakeLists.txt @@ -7,6 +7,8 @@ if (APPLE) set_target_properties(${TARGET_NAME} PROPERTIES INSTALL_RPATH "@executable_path/../Frameworks") endif () +setup_memory_debugger() + # link in the shared libraries link_hifi_libraries( audio avatars octree gpu model fbx entities diff --git a/domain-server/CMakeLists.txt b/domain-server/CMakeLists.txt index 2ce537a5a0..c1e275e4d3 100644 --- a/domain-server/CMakeLists.txt +++ b/domain-server/CMakeLists.txt @@ -14,6 +14,8 @@ if (APPLE) set_target_properties(${TARGET_NAME} PROPERTIES INSTALL_RPATH "@executable_path/../Frameworks") endif () +setup_memory_debugger() + # TODO: find a solution that will handle web file changes in resources on windows without a re-build. # Currently the resources are only copied on post-build. If one is changed but the domain-server is not, they will # not be re-copied. This is worked-around on OS X/UNIX by using a symlink. diff --git a/ice-server/CMakeLists.txt b/ice-server/CMakeLists.txt index e5bdffe2e2..07b90b369e 100644 --- a/ice-server/CMakeLists.txt +++ b/ice-server/CMakeLists.txt @@ -18,5 +18,7 @@ endif () include_directories(SYSTEM "${OPENSSL_INCLUDE_DIR}") +setup_memory_debugger() + # append OpenSSL to our list of libraries to link target_link_libraries(${TARGET_NAME} ${OPENSSL_LIBRARIES}) diff --git a/interface/CMakeLists.txt b/interface/CMakeLists.txt index 71341f3f11..fe42b1432b 100644 --- a/interface/CMakeLists.txt +++ b/interface/CMakeLists.txt @@ -356,6 +356,8 @@ if (ANDROID) qt_create_apk() endif () +setup_memory_debugger() + add_dependency_external_projects(GifCreator) find_package(GifCreator REQUIRED) target_include_directories(${TARGET_NAME} PUBLIC ${GIFCREATOR_INCLUDE_DIRS}) diff --git a/libraries/gl/CMakeLists.txt b/libraries/gl/CMakeLists.txt index fd3197410b..0a0ca2fc5f 100644 --- a/libraries/gl/CMakeLists.txt +++ b/libraries/gl/CMakeLists.txt @@ -1,5 +1,6 @@ set(TARGET_NAME gl) setup_hifi_library(OpenGL Qml Quick) +setup_memory_debugger() link_hifi_libraries(shared networking) target_opengl() diff --git a/tests/controllers/CMakeLists.txt b/tests/controllers/CMakeLists.txt index 3aac4db0a8..3221070837 100644 --- a/tests/controllers/CMakeLists.txt +++ b/tests/controllers/CMakeLists.txt @@ -5,6 +5,8 @@ set(TARGET_NAME controllers-test) setup_hifi_project(Script Qml) set_target_properties(${TARGET_NAME} PROPERTIES FOLDER "Tests/manual-tests/") +setup_memory_debugger() + # link in the shared libraries link_hifi_libraries(shared gl script-engine plugins render-utils ui-plugins input-plugins display-plugins controllers) @@ -16,4 +18,4 @@ if (WIN32) target_link_libraries(${TARGET_NAME} ${OPENVR_LIBRARIES}) endif() -package_libraries_for_deployment() \ No newline at end of file +package_libraries_for_deployment() diff --git a/tests/qt59/CMakeLists.txt b/tests/qt59/CMakeLists.txt index 32cc125ecf..e0e8138a1e 100644 --- a/tests/qt59/CMakeLists.txt +++ b/tests/qt59/CMakeLists.txt @@ -1,10 +1,12 @@ set(TARGET_NAME qt59) - + if (WIN32) SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} /ignore:4049 /ignore:4217") endif() +setup_memory_debugger() + # This is not a testcase -- just set it up as a regular hifi project setup_hifi_project(Gui) set_target_properties(${TARGET_NAME} PROPERTIES FOLDER "Tests/manual-tests/") diff --git a/tests/recording/CMakeLists.txt b/tests/recording/CMakeLists.txt index 4e881fcbd9..b5b1e6a54e 100644 --- a/tests/recording/CMakeLists.txt +++ b/tests/recording/CMakeLists.txt @@ -2,6 +2,7 @@ set(TARGET_NAME recording-test) # This is not a testcase -- just set it up as a regular hifi project setup_hifi_project(Test) set_target_properties(${TARGET_NAME} PROPERTIES FOLDER "Tests/manual-tests/") +setup_memory_debugger() link_hifi_libraries(shared recording) package_libraries_for_deployment() diff --git a/tools/vhacd-util/CMakeLists.txt b/tools/vhacd-util/CMakeLists.txt index 810d13ffd7..c28aa9efa4 100644 --- a/tools/vhacd-util/CMakeLists.txt +++ b/tools/vhacd-util/CMakeLists.txt @@ -8,6 +8,8 @@ find_package(VHACD REQUIRED) target_include_directories(${TARGET_NAME} PUBLIC ${VHACD_INCLUDE_DIRS}) target_link_libraries(${TARGET_NAME} ${VHACD_LIBRARIES}) +setup_memory_debugger() + if (UNIX AND NOT APPLE) include(FindOpenMP) if(OPENMP_FOUND) From 9ce173f682cf04a8fb3e32bd2f1d65503e639884 Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Thu, 22 Jun 2017 10:56:08 -0700 Subject: [PATCH 23/42] missed a few --- tools/ac-client/CMakeLists.txt | 1 + tools/atp-get/CMakeLists.txt | 1 + tools/skeleton-dump/CMakeLists.txt | 1 + 3 files changed, 3 insertions(+) diff --git a/tools/ac-client/CMakeLists.txt b/tools/ac-client/CMakeLists.txt index 9e623b02e9..24eeadba9c 100644 --- a/tools/ac-client/CMakeLists.txt +++ b/tools/ac-client/CMakeLists.txt @@ -1,3 +1,4 @@ set(TARGET_NAME ac-client) setup_hifi_project(Core Widgets) +setup_memory_debugger() link_hifi_libraries(shared networking) diff --git a/tools/atp-get/CMakeLists.txt b/tools/atp-get/CMakeLists.txt index b1646dc023..75f92b787d 100644 --- a/tools/atp-get/CMakeLists.txt +++ b/tools/atp-get/CMakeLists.txt @@ -1,3 +1,4 @@ set(TARGET_NAME atp-get) setup_hifi_project(Core Widgets) +setup_memory_debugger() link_hifi_libraries(shared networking) diff --git a/tools/skeleton-dump/CMakeLists.txt b/tools/skeleton-dump/CMakeLists.txt index 04d450d9c2..bb2fe24f51 100644 --- a/tools/skeleton-dump/CMakeLists.txt +++ b/tools/skeleton-dump/CMakeLists.txt @@ -1,4 +1,5 @@ set(TARGET_NAME skeleton-dump) setup_hifi_project(Core Widgets) +setup_memory_debugger() link_hifi_libraries(shared fbx model gpu gl animation) From 3184d4e9d13ae105e531fffff9a4e61959cb48be Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 22 Jun 2017 12:17:22 -0700 Subject: [PATCH 24/42] Puck attach only shows available tracked objects. --- scripts/developer/tests/puck-attach.js | 35 +++++++++++++++++++++++--- 1 file changed, 31 insertions(+), 4 deletions(-) diff --git a/scripts/developer/tests/puck-attach.js b/scripts/developer/tests/puck-attach.js index 807977794d..7fe13e6ba1 100644 --- a/scripts/developer/tests/puck-attach.js +++ b/scripts/developer/tests/puck-attach.js @@ -13,7 +13,8 @@ Script.include("/~/system/libraries/Xform.js"); (function() { // BEGIN LOCAL_SCOPE var TABLET_BUTTON_NAME = "PUCKTACH"; -var HTML_URL = "https://s3.amazonaws.com/hifi-public/tony/html/puck-attach.html"; +// var HTML_URL = "https://s3.amazonaws.com/hifi-public/tony/html/puck-attach.html"; +var HTML_URL = "file:///C:/msys64/home/anthony/code/hifi/examples/html/puck-attach.html"; var tablet = Tablet.getTablet("com.highfidelity.interface.tablet.system"); var tabletButton = tablet.addButton({ @@ -40,6 +41,14 @@ function onScreenChanged(type, url) { if (!shown) { // hook up to event bridge tablet.webEventReceived.connect(onWebEventReceived); + + Script.setTimeout(function () { + // send available tracked objects to the html running in the tablet. + var availableTrackedObjects = getAvailableTrackedObjects(); + tablet.emitScriptEvent(JSON.stringify(availableTrackedObjects)); + + print("PUCK-ATTACH: availableTrackedObjects = " + JSON.stringify(availableTrackedObjects)); + }, 1000); // wait 1 sec before sending.. } shown = true; } else { @@ -54,6 +63,24 @@ function onScreenChanged(type, url) { tablet.screenChanged.connect(onScreenChanged); +function indexToTrackedObjectName(index) { + return "TrackedObject" + pad(index, 2); +} + +function getAvailableTrackedObjects() { + var available = []; + var NUM_TRACKED_OBJECTS = 16; + var i; + for (i = 0; i < NUM_TRACKED_OBJECTS; i++) { + var key = indexToTrackedObjectName(i); + var pose = Controller.getPoseValue(Controller.Hardware.Vive[key]); + if (pose && pose.valid) { + available.push(i); + } + } + return available; +} + function attach(obj) { attachedEntity = Entities.addEntity({ type: "Model", @@ -64,7 +91,7 @@ function attach(obj) { var localPos = {x: Number(obj.posx), y: Number(obj.posy), z: Number(obj.posz)}; var localRot = Quat.fromVec3Degrees({x: Number(obj.rotx), y: Number(obj.roty), z: Number(obj.rotz)}); attachedObj.localXform = new Xform(localRot, localPos); - var key = "TrackedObject" + pad(attachedObj.puckno, 2); + var key = indexToTrackedObjectName(Number(attachedObj.puckno)); attachedObj.key = key; print("PUCK-ATTACH: attachedObj = " + JSON.stringify(attachedObj)); @@ -83,8 +110,8 @@ function remove() { } function pad(num, size) { - var s = "000000000" + num; - return s.substr(s.length-size); + var tempString = "000000000" + num; + return tempString.substr(tempString.length - size); } function update(dt) { From e531468ac7dacbe5b99e5292141a8f0158f13e8a Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 22 Jun 2017 13:14:47 -0700 Subject: [PATCH 25/42] Fix for html url --- scripts/developer/tests/puck-attach.js | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/scripts/developer/tests/puck-attach.js b/scripts/developer/tests/puck-attach.js index 7fe13e6ba1..00d2ca5fa3 100644 --- a/scripts/developer/tests/puck-attach.js +++ b/scripts/developer/tests/puck-attach.js @@ -13,8 +13,7 @@ Script.include("/~/system/libraries/Xform.js"); (function() { // BEGIN LOCAL_SCOPE var TABLET_BUTTON_NAME = "PUCKTACH"; -// var HTML_URL = "https://s3.amazonaws.com/hifi-public/tony/html/puck-attach.html"; -var HTML_URL = "file:///C:/msys64/home/anthony/code/hifi/examples/html/puck-attach.html"; +var HTML_URL = "https://s3.amazonaws.com/hifi-public/tony/html/puck-attach.html"; var tablet = Tablet.getTablet("com.highfidelity.interface.tablet.system"); var tabletButton = tablet.addButton({ From a053a1da4ba402cbcae380ba91c837d5a08a866c Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Thu, 22 Jun 2017 13:50:46 -0700 Subject: [PATCH 26/42] more HIFI_MEMORY_DEBUGGING fixes --- libraries/gl/CMakeLists.txt | 1 - tests/entities/CMakeLists.txt | 2 +- tools/ice-client/CMakeLists.txt | 1 + 3 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/gl/CMakeLists.txt b/libraries/gl/CMakeLists.txt index 0a0ca2fc5f..fd3197410b 100644 --- a/libraries/gl/CMakeLists.txt +++ b/libraries/gl/CMakeLists.txt @@ -1,6 +1,5 @@ set(TARGET_NAME gl) setup_hifi_library(OpenGL Qml Quick) -setup_memory_debugger() link_hifi_libraries(shared networking) target_opengl() diff --git a/tests/entities/CMakeLists.txt b/tests/entities/CMakeLists.txt index 448ea83956..080ae7cdd9 100644 --- a/tests/entities/CMakeLists.txt +++ b/tests/entities/CMakeLists.txt @@ -3,7 +3,7 @@ set(TARGET_NAME "entities-test") # This is not a testcase -- just set it up as a regular hifi project setup_hifi_project(Network Script) - +setup_memory_debugger() set_target_properties(${TARGET_NAME} PROPERTIES FOLDER "Tests/manual-tests/") # link in the shared libraries diff --git a/tools/ice-client/CMakeLists.txt b/tools/ice-client/CMakeLists.txt index a80145974c..ae42d79f7e 100644 --- a/tools/ice-client/CMakeLists.txt +++ b/tools/ice-client/CMakeLists.txt @@ -1,3 +1,4 @@ set(TARGET_NAME ice-client) setup_hifi_project(Core Widgets) +setup_memory_debugger() link_hifi_libraries(shared networking) From 5af07dfb627e5803bf36e295020c7f9bd6120843 Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Fri, 23 Jun 2017 08:16:48 -0700 Subject: [PATCH 27/42] more memory-debugger --- interface/CMakeLists.txt | 4 ++-- tests/gpu-test/CMakeLists.txt | 1 + tests/render-perf/CMakeLists.txt | 2 ++ tests/render-texture-load/CMakeLists.txt | 2 ++ tests/render-utils/CMakeLists.txt | 2 ++ tests/shaders/CMakeLists.txt | 2 ++ 6 files changed, 11 insertions(+), 2 deletions(-) diff --git a/interface/CMakeLists.txt b/interface/CMakeLists.txt index fe42b1432b..dcb1cacef9 100644 --- a/interface/CMakeLists.txt +++ b/interface/CMakeLists.txt @@ -4,6 +4,8 @@ project(${TARGET_NAME}) # set a default root dir for each of our optional externals if it was not passed set(OPTIONAL_EXTERNALS "LeapMotion") +setup_memory_debugger() + foreach(EXTERNAL ${OPTIONAL_EXTERNALS}) string(TOUPPER ${EXTERNAL} ${EXTERNAL}_UPPERCASE) if (NOT ${${EXTERNAL}_UPPERCASE}_ROOT_DIR) @@ -356,8 +358,6 @@ if (ANDROID) qt_create_apk() endif () -setup_memory_debugger() - add_dependency_external_projects(GifCreator) find_package(GifCreator REQUIRED) target_include_directories(${TARGET_NAME} PUBLIC ${GIFCREATOR_INCLUDE_DIRS}) diff --git a/tests/gpu-test/CMakeLists.txt b/tests/gpu-test/CMakeLists.txt index c37e36b53b..d73d7a111d 100644 --- a/tests/gpu-test/CMakeLists.txt +++ b/tests/gpu-test/CMakeLists.txt @@ -2,6 +2,7 @@ set(TARGET_NAME gpu-test) AUTOSCRIBE_SHADER_LIB(gpu model render-utils) # This is not a testcase -- just set it up as a regular hifi project setup_hifi_project(Quick Gui OpenGL Script Widgets) +setup_memory_debugger() set_target_properties(${TARGET_NAME} PROPERTIES FOLDER "Tests/manual-tests/") link_hifi_libraries(networking gl gpu gpu-gl procedural shared fbx model model-networking animation script-engine render render-utils octree image ktx) package_libraries_for_deployment() diff --git a/tests/render-perf/CMakeLists.txt b/tests/render-perf/CMakeLists.txt index a8e4ab286c..b6989a57b7 100644 --- a/tests/render-perf/CMakeLists.txt +++ b/tests/render-perf/CMakeLists.txt @@ -5,6 +5,8 @@ if (WIN32) SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} /ignore:4049 /ignore:4217") endif() +setup_memory_debugger() + # This is not a testcase -- just set it up as a regular hifi project setup_hifi_project(Quick Gui OpenGL) set_target_properties(${TARGET_NAME} PROPERTIES FOLDER "Tests/manual-tests/") diff --git a/tests/render-texture-load/CMakeLists.txt b/tests/render-texture-load/CMakeLists.txt index 1f0c0a069a..30030914ab 100644 --- a/tests/render-texture-load/CMakeLists.txt +++ b/tests/render-texture-load/CMakeLists.txt @@ -5,6 +5,8 @@ if (WIN32) SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} /ignore:4049 /ignore:4217") endif() +setup_memory_debugger() + # This is not a testcase -- just set it up as a regular hifi project setup_hifi_project(Quick Gui OpenGL) set_target_properties(${TARGET_NAME} PROPERTIES FOLDER "Tests/manual-tests/") diff --git a/tests/render-utils/CMakeLists.txt b/tests/render-utils/CMakeLists.txt index 5ec6a28b5c..4944ad2cbc 100644 --- a/tests/render-utils/CMakeLists.txt +++ b/tests/render-utils/CMakeLists.txt @@ -5,6 +5,8 @@ set(TARGET_NAME render-utils-test) setup_hifi_project(Quick Gui OpenGL) set_target_properties(${TARGET_NAME} PROPERTIES FOLDER "Tests/manual-tests/") +setup_memory_debugger() + # link in the shared libraries link_hifi_libraries(render-utils gl gpu gpu-gl shared) target_link_libraries(${TARGET_NAME} ${CMAKE_THREAD_LIBS_INIT}) diff --git a/tests/shaders/CMakeLists.txt b/tests/shaders/CMakeLists.txt index 5b38f473e8..bab1e0dcdc 100644 --- a/tests/shaders/CMakeLists.txt +++ b/tests/shaders/CMakeLists.txt @@ -5,6 +5,8 @@ set(TARGET_NAME shaders-test) setup_hifi_project(Quick Gui OpenGL) set_target_properties(${TARGET_NAME} PROPERTIES FOLDER "Tests/manual-tests/") +setup_memory_debugger() + # link in the shared libraries link_hifi_libraries(shared octree gl gpu gpu-gl model render fbx networking entities script-engine physics From 54af6af651117bbc0c4703abda38b14ff56b505a Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 23 Jun 2017 10:38:21 -0700 Subject: [PATCH 28/42] Fix for pole vector stability and knee pole vector computation --- .../animation/src/AnimInverseKinematics.cpp | 45 +++++++++----- libraries/animation/src/Rig.cpp | 62 ++++++++++++------- libraries/animation/src/Rig.h | 2 +- 3 files changed, 71 insertions(+), 38 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 3ea8937eec..e2e0d72d66 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -530,22 +530,33 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const if (dLen > EPSILON) { glm::vec3 dUnit = d / dLen; glm::vec3 e = midPose.xformVector(target.getPoleReferenceVector()); - glm::vec3 p = target.getPoleVector(); glm::vec3 eProj = e - glm::dot(e, dUnit) * dUnit; - glm::vec3 pProj = p - glm::dot(p, dUnit) * dUnit; float eProjLen = glm::length(eProj); - float pProjLen = glm::length(pProj); - if (eProjLen > EPSILON && pProjLen > EPSILON) { + const float MIN_EPROJ_LEN = 0.5f; + if (eProjLen < MIN_EPROJ_LEN) { + glm::vec3 midPoint = topPose.trans() + d * 0.5f; + e = midPose.trans() - midPoint; + eProj = e - glm::dot(e, dUnit) * dUnit; + eProjLen = glm::length(eProj); + } + + glm::vec3 p = target.getPoleVector(); + glm::vec3 pProj = p - glm::dot(p, dUnit) * dUnit; + float pProjLen = glm::length(pProj); + + if (eProjLen > EPSILON && pProjLen > EPSILON) { // as pProjLen become orthognal to d, reduce the amount of rotation. float magnitude = easeOutExpo(pProjLen); - float dot = glm::clamp(glm::dot(eProj / eProjLen, pProj / pProjLen), 0.0f, 1.0f); float theta = acosf(dot); glm::vec3 cross = glm::cross(eProj, pProj); - float crossLen = glm::length(cross); - if (crossLen > EPSILON) { - glm::vec3 axis = cross / crossLen; + const float MIN_ADJUSTMENT_ANGLE = 0.001745f; // 0.1 degree + if (theta > MIN_ADJUSTMENT_ANGLE) { + glm::vec3 axis = dUnit; + if (glm::dot(cross, dUnit) < 0) { + axis = -dUnit; + } poleRot = glm::angleAxis(magnitude * theta, axis); } } @@ -562,8 +573,17 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const glm::vec3 dUnit = d / dLen; glm::vec3 e = midPose.xformVector(target.getPoleReferenceVector()); - glm::vec3 p = target.getPoleVector(); glm::vec3 eProj = e - glm::dot(e, dUnit) * dUnit; + float eProjLen = glm::length(eProj); + const float MIN_EPROJ_LEN = 0.5f; + if (eProjLen < MIN_EPROJ_LEN) { + glm::vec3 midPoint = topPose.trans() + d * 0.5f; + e = midPose.trans() - midPoint; + eProj = e - glm::dot(e, dUnit) * dUnit; + eProjLen = glm::length(eProj); + } + + glm::vec3 p = target.getPoleVector(); glm::vec3 pProj = p - glm::dot(p, dUnit) * dUnit; const float PROJ_VECTOR_LEN = 10.0f; @@ -573,11 +593,8 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const geomToWorldPose.xformPoint(topPose.trans()), YELLOW); DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint), - geomToWorldPose.xformPoint(midPoint + PROJ_VECTOR_LEN * glm::normalize(eProj)), + geomToWorldPose.xformPoint(midPoint + PROJ_VECTOR_LEN * glm::normalize(e)), RED); - DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint), - geomToWorldPose.xformPoint(midPoint + PROJ_VECTOR_LEN * glm::normalize(pProj)), - GREEN); DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint), geomToWorldPose.xformPoint(midPoint + POLE_VECTOR_LEN * glm::normalize(p)), BLUE); @@ -1060,7 +1077,7 @@ void AnimInverseKinematics::initConstraints() { // y | // | | // | O---O---O RightUpLeg - // z | | Hips2 | + // z | | Hips | // \ | | | // \| | | // x -----+ O O RightLeg diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 51480e8ef1..8466b79d1d 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1265,14 +1265,14 @@ void Rig::updateFeet(bool leftFootEnabled, bool rightFootEnabled, const AnimPose int hipsIndex = indexOfJoint("Hips"); if (leftFootEnabled) { - glm::vec3 footPosition = leftFootPose.trans(); - glm::quat footRotation = leftFootPose.rot(); - _animVars.set("leftFootPosition", footPosition); - _animVars.set("leftFootRotation", footRotation); + _animVars.set("leftFootPosition", leftFootPose.trans()); + _animVars.set("leftFootRotation", leftFootPose.rot()); _animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition); int footJointIndex = _animSkeleton->nameToJointIndex("LeftFoot"); - glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, footRotation, hipsIndex); + int kneeJointIndex = _animSkeleton->nameToJointIndex("LeftLeg"); + int upLegJointIndex = _animSkeleton->nameToJointIndex("LeftUpLeg"); + glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, kneeJointIndex, upLegJointIndex, hipsIndex, leftFootPose); // smooth toward desired pole vector from previous pole vector... to reduce jitter if (!_prevLeftFootPoleVectorValid) { @@ -1295,14 +1295,14 @@ void Rig::updateFeet(bool leftFootEnabled, bool rightFootEnabled, const AnimPose } if (rightFootEnabled) { - glm::vec3 footPosition = rightFootPose.trans(); - glm::quat footRotation = rightFootPose.rot(); - _animVars.set("rightFootPosition", footPosition); - _animVars.set("rightFootRotation", footRotation); + _animVars.set("rightFootPosition", rightFootPose.trans()); + _animVars.set("rightFootRotation", rightFootPose.rot()); _animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition); int footJointIndex = _animSkeleton->nameToJointIndex("RightFoot"); - glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, footRotation, hipsIndex); + int kneeJointIndex = _animSkeleton->nameToJointIndex("RightLeg"); + int upLegJointIndex = _animSkeleton->nameToJointIndex("RightUpLeg"); + glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, kneeJointIndex, upLegJointIndex, hipsIndex, rightFootPose); // smooth toward desired pole vector from previous pole vector... to reduce jitter if (!_prevRightFootPoleVectorValid) { @@ -1375,20 +1375,23 @@ glm::vec3 Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIn AnimPose handPose = _externalPoseSet._absolutePoses[handIndex]; AnimPose elbowPose = _externalPoseSet._absolutePoses[elbowIndex]; AnimPose armPose = _externalPoseSet._absolutePoses[armIndex]; + + // ray from hand to arm. glm::vec3 d = glm::normalize(handPose.trans() - armPose.trans()); float sign = isLeft ? 1.0f : -1.0f; + // form a plane normal to the hips x-axis. glm::vec3 n = hipsPose.rot() * Vectors::UNIT_X; glm::vec3 y = hipsPose.rot() * Vectors::UNIT_Y; - // project d onto n. + + // project d onto this plane glm::vec3 dProj = d - glm::dot(d, n) * n; + // give dProj a bit of offset away from the body. float avatarScale = extractUniformScale(_modelOffset); const float LATERAL_OFFSET = 0.333f * avatarScale; const float VERTICAL_OFFSET = -0.333f * avatarScale; - - // give dProj a bit of offset away from the body. glm::vec3 dProjWithOffset = dProj + sign * LATERAL_OFFSET * n + y * VERTICAL_OFFSET; // rotate dProj by 90 degrees to get the poleVector. @@ -1402,19 +1405,32 @@ glm::vec3 Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIn return glm::normalize(poleAdjust * poleVector); } -glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, const glm::quat& footTargetOrientation, int hipsIndex) const { +glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, int kneeIndex, int upLegIndex, int hipsIndex, const AnimPose& targetFootPose) const { - AnimPose defaultPose = _animSkeleton->getAbsoluteDefaultPose(footJointIndex); - glm::vec3 localForward = glm::inverse(defaultPose.rot()) * Vectors::UNIT_Z; - glm::vec3 footForward = footTargetOrientation * localForward; + AnimPose hipsPose = _externalPoseSet._absolutePoses[hipsIndex]; + AnimPose footPose = targetFootPose; + AnimPose kneePose = _externalPoseSet._absolutePoses[kneeIndex]; + AnimPose upLegPose = _externalPoseSet._absolutePoses[upLegIndex]; - // compute the forward direction of the hips. - glm::quat hipsRotation = _externalPoseSet._absolutePoses[hipsIndex].rot(); - glm::vec3 hipsForward = hipsRotation * Vectors::UNIT_Z; + // ray from foot to upLeg + glm::vec3 d = glm::normalize(footPose.trans() - upLegPose.trans()); - // blend between the hips and the foot. - const float FOOT_TO_HIPS_BLEND_FACTOR = 0.5f; - return glm::normalize(lerp(footForward, hipsForward, FOOT_TO_HIPS_BLEND_FACTOR)); + // form a plane normal to the hips x-axis + glm::vec3 n = hipsPose.rot() * Vectors::UNIT_X; + glm::vec3 y = hipsPose.rot() * Vectors::UNIT_Y; + + // project d onto this plane + glm::vec3 dProj = d - glm::dot(d, n) * n; + + // rotate dProj by 90 degrees to get the poleVector. + glm::vec3 poleVector = glm::angleAxis(-PI / 2.0f, n) * dProj; + + // blend the foot oreintation into the pole vector + glm::quat kneeToFootDelta = footPose.rot() * glm::inverse(kneePose.rot()); + const float WRIST_POLE_ADJUST_FACTOR = 0.5f; + glm::quat poleAdjust = quatLerp(Quaternions::IDENTITY, kneeToFootDelta, WRIST_POLE_ADJUST_FACTOR); + + return glm::normalize(poleAdjust * poleVector); } void Rig::updateFromControllerParameters(const ControllerParameters& params, float dt) { diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 413b598079..c17a7b9c8f 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -239,7 +239,7 @@ protected: void calcAnimAlpha(float speed, const std::vector& referenceSpeeds, float* alphaOut) const; glm::vec3 calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, int hipsIndex, bool isLeft) const; - glm::vec3 calculateKneePoleVector(int footJointIndex, const glm::quat& footTargetOrientation, int hipsIndex) const; + glm::vec3 calculateKneePoleVector(int footJointIndex, int kneeJoint, int upLegIndex, int hipsIndex, const AnimPose& targetFootPose) const; AnimPose _modelOffset; // model to rig space AnimPose _geometryOffset; // geometry to model space (includes unit offset & fst offsets) From f3206106812ceb2e376f6dd41c7d9aa3d8a8da64 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 23 Jun 2017 14:06:00 -0700 Subject: [PATCH 29/42] opened up shoulder constraint + other fixes * bent elbows will be away from the body a bit more. * sped up smoothing of pole vectors --- libraries/animation/src/AnimInverseKinematics.cpp | 6 ++++-- libraries/animation/src/Rig.cpp | 6 +++--- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index e2e0d72d66..0395d4d391 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -1122,7 +1122,9 @@ void AnimInverseKinematics::initConstraints() { if (0 == baseName.compare("Arm", Qt::CaseSensitive)) { SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot()); - stConstraint->setTwistLimits(-PI / 2.0f, PI / 2.0f); + //stConstraint->setTwistLimits(-PI / 2.0f, PI / 2.0f); + const float TWIST_LIMIT = 5.0f * PI / 8.0f; + stConstraint->setTwistLimits(-TWIST_LIMIT, TWIST_LIMIT); /* KEEP THIS CODE for future experimentation // these directions are approximate swing limits in root-frame @@ -1148,7 +1150,7 @@ void AnimInverseKinematics::initConstraints() { // simple cone std::vector minDots; - const float MAX_HAND_SWING = PI / 2.0f; + const float MAX_HAND_SWING = 5.0f * PI / 8.0f; minDots.push_back(cosf(MAX_HAND_SWING)); stConstraint->setSwingLimits(minDots); diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 8466b79d1d..53e3f3ddc0 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1089,7 +1089,7 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab const bool LEFT_HAND = true; const bool RIGHT_HAND = false; - const float ELBOW_POLE_VECTOR_BLEND_FACTOR = 0.9f; + const float ELBOW_POLE_VECTOR_BLEND_FACTOR = 0.95f; if (leftHandEnabled) { if (!_isLeftHandControlled) { @@ -1260,7 +1260,7 @@ void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnab void Rig::updateFeet(bool leftFootEnabled, bool rightFootEnabled, const AnimPose& leftFootPose, const AnimPose& rightFootPose) { - const float KNEE_POLE_VECTOR_BLEND_FACTOR = 0.9f; + const float KNEE_POLE_VECTOR_BLEND_FACTOR = 0.95f; int hipsIndex = indexOfJoint("Hips"); @@ -1390,7 +1390,7 @@ glm::vec3 Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIn // give dProj a bit of offset away from the body. float avatarScale = extractUniformScale(_modelOffset); - const float LATERAL_OFFSET = 0.333f * avatarScale; + const float LATERAL_OFFSET = 1.0f * avatarScale; const float VERTICAL_OFFSET = -0.333f * avatarScale; glm::vec3 dProjWithOffset = dProj + sign * LATERAL_OFFSET * n + y * VERTICAL_OFFSET; From b8c638b2b7214d5e7f508366443a33761dcf8ca1 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 23 Jun 2017 14:36:59 -0700 Subject: [PATCH 30/42] warning fixes --- libraries/animation/src/AnimInverseKinematics.cpp | 2 -- libraries/animation/src/Rig.cpp | 1 - 2 files changed, 3 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 0395d4d391..3e948a4f5b 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -584,8 +584,6 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const } glm::vec3 p = target.getPoleVector(); - glm::vec3 pProj = p - glm::dot(p, dUnit) * dUnit; - const float PROJ_VECTOR_LEN = 10.0f; const float POLE_VECTOR_LEN = 100.0f; glm::vec3 midPoint = (basePose.trans() + topPose.trans()) * 0.5f; diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 53e3f3ddc0..3f63f226e7 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1417,7 +1417,6 @@ glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, int kneeIndex, int up // form a plane normal to the hips x-axis glm::vec3 n = hipsPose.rot() * Vectors::UNIT_X; - glm::vec3 y = hipsPose.rot() * Vectors::UNIT_Y; // project d onto this plane glm::vec3 dProj = d - glm::dot(d, n) * n; From aabbcfd23dafecd99beb85a6c3ab3cf7a7e6b3dc Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 23 Jun 2017 16:47:53 -0700 Subject: [PATCH 31/42] code review feedback --- scripts/developer/tests/puck-attach.js | 20 ++++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/scripts/developer/tests/puck-attach.js b/scripts/developer/tests/puck-attach.js index 00d2ca5fa3..2d0a2e6d02 100644 --- a/scripts/developer/tests/puck-attach.js +++ b/scripts/developer/tests/puck-attach.js @@ -5,6 +5,18 @@ // Distributed under the Apache License, Version 2.0. // See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html // +// When this script is running, a new app button, named "PUCKTACH", will be added to the toolbar/tablet. +// Click this app to bring up the puck attachment panel. This panel contains the following fields. +// +// * Tracked Object - A drop down list of all the available pucks found. If no pucks are found this list will only have a single NONE entry. +// Closing and re-opening the app will refresh this list. +// * Model URL - A model url of the model you wish to be attached to the specified puck. +// * Position X, Y, Z - used to apply an offset between the puck and the attached model. +// * Rot X, Y, Z - used to apply euler angle offsets, in degrees, between the puck and the attached model. +// * Create Attachment - when this button is pressed a new Entity will be created at the specified puck's location. +// If a puck atttachment already exists, it will be destroyed before the new entity is created. +// * Destroy Attachmetn - destroies the entity attached to the puck. +// /* eslint indent: ["error", 4, { "outerIIFEBody": 0 }] */ /* global Xform */ @@ -46,7 +58,7 @@ function onScreenChanged(type, url) { var availableTrackedObjects = getAvailableTrackedObjects(); tablet.emitScriptEvent(JSON.stringify(availableTrackedObjects)); - print("PUCK-ATTACH: availableTrackedObjects = " + JSON.stringify(availableTrackedObjects)); + // print("PUCK-ATTACH: availableTrackedObjects = " + JSON.stringify(availableTrackedObjects)); }, 1000); // wait 1 sec before sending.. } shown = true; @@ -93,10 +105,10 @@ function attach(obj) { var key = indexToTrackedObjectName(Number(attachedObj.puckno)); attachedObj.key = key; - print("PUCK-ATTACH: attachedObj = " + JSON.stringify(attachedObj)); + // print("PUCK-ATTACH: attachedObj = " + JSON.stringify(attachedObj)); Script.update.connect(update); - update(0.001); + update(); } function remove() { @@ -113,7 +125,7 @@ function pad(num, size) { return tempString.substr(tempString.length - size); } -function update(dt) { +function update() { if (attachedEntity && attachedObj && Controller.Hardware.Vive) { var pose = Controller.getPoseValue(Controller.Hardware.Vive[attachedObj.key]); var avatarXform = new Xform(MyAvatar.orientation, MyAvatar.position); From b546d977f4ec3ff2874e62901f8355e7959e9121 Mon Sep 17 00:00:00 2001 From: David Rowe Date: Sat, 24 Jun 2017 12:07:20 +1200 Subject: [PATCH 32/42] "EZRECORD" app script and button --- scripts/developer/EZrecord.js | 60 +++++++++++++++++++++++++++++++++++ 1 file changed, 60 insertions(+) create mode 100644 scripts/developer/EZrecord.js diff --git a/scripts/developer/EZrecord.js b/scripts/developer/EZrecord.js new file mode 100644 index 0000000000..ae397afb36 --- /dev/null +++ b/scripts/developer/EZrecord.js @@ -0,0 +1,60 @@ +"use strict"; + +// +// EZrecord.js +// +// Created by David Rowe on 24 Jun 2017. +// Copyright 2017 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +(function () { + + var APP_NAME = "EZRECORD", + APP_ICON_INACTIVE = "icons/tablet-icons/avatar-record-i.svg", + APP_ICON_ACTIVE = "icons/tablet-icons/avatar-record-a.svg", + tablet, + button; + + function onButtonClicked() { + // TODO + print("Clicked"); + } + + function setUp() { + tablet = Tablet.getTablet("com.highfidelity.interface.tablet.system"); + if (!tablet) { + return; + } + + // Tablet/toolbar button. + button = tablet.addButton({ + icon: APP_ICON_INACTIVE, + activeIcon: APP_ICON_ACTIVE, + text: APP_NAME, + isActive: false + }); + if (button) { + button.clicked.connect(onButtonClicked); + } + } + + function tearDown() { + if (!tablet) { + return; + } + + if (button) { + button.clicked.disconnect(onButtonClicked); + tablet.removeButton(button); + button = null; + } + + tablet = null; + } + + setUp(); + Script.scriptEnding.connect(tearDown); +}()); From ced9473eb34d624889206d54dfee1c9024ec8a8b Mon Sep 17 00:00:00 2001 From: David Rowe Date: Sat, 24 Jun 2017 12:49:45 +1200 Subject: [PATCH 33/42] Icon toggling and keyboard shortcut --- scripts/developer/EZrecord.js | 27 ++++++++++++++++++++++++--- 1 file changed, 24 insertions(+), 3 deletions(-) diff --git a/scripts/developer/EZrecord.js b/scripts/developer/EZrecord.js index ae397afb36..d28c182ea0 100644 --- a/scripts/developer/EZrecord.js +++ b/scripts/developer/EZrecord.js @@ -15,12 +15,25 @@ var APP_NAME = "EZRECORD", APP_ICON_INACTIVE = "icons/tablet-icons/avatar-record-i.svg", APP_ICON_ACTIVE = "icons/tablet-icons/avatar-record-a.svg", + SHORTCUT_KEY = "r", // Ctrl modifier is assumed. tablet, - button; + button, + isRecording = false; + + function toggleRecording() { + isRecording = !isRecording; + button.editProperties({ isActive: isRecording }); + // TODO: Start/cancel/finish recording. + } + + function onKeyPressEvent(event) { + if (event.isControl && event.text === SHORTCUT_KEY && !event.isMeta && !event.isAlt && !event.isAutoRepeat) { + toggleRecording(); + } + } function onButtonClicked() { - // TODO - print("Clicked"); + toggleRecording(); } function setUp() { @@ -39,9 +52,15 @@ if (button) { button.clicked.connect(onButtonClicked); } + + Controller.keyPressEvent.connect(onKeyPressEvent); } function tearDown() { + if (isRecording) { + // TODO: Cancel recording. + } + if (!tablet) { return; } @@ -53,6 +72,8 @@ } tablet = null; + + Controller.keyPressEvent.disconnect(onKeyPressEvent); } setUp(); From 1ddae1c61ad02c82538d0360cfebbccd937a2c5a Mon Sep 17 00:00:00 2001 From: David Rowe Date: Sat, 24 Jun 2017 13:19:25 +1200 Subject: [PATCH 34/42] Change keyboard shortcut to avoid conflict --- scripts/developer/EZrecord.js | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/scripts/developer/EZrecord.js b/scripts/developer/EZrecord.js index d28c182ea0..bb173343df 100644 --- a/scripts/developer/EZrecord.js +++ b/scripts/developer/EZrecord.js @@ -15,7 +15,7 @@ var APP_NAME = "EZRECORD", APP_ICON_INACTIVE = "icons/tablet-icons/avatar-record-i.svg", APP_ICON_ACTIVE = "icons/tablet-icons/avatar-record-a.svg", - SHORTCUT_KEY = "r", // Ctrl modifier is assumed. + SHORTCUT_KEY = "r", // Alt modifier is assumed. tablet, button, isRecording = false; @@ -27,7 +27,7 @@ } function onKeyPressEvent(event) { - if (event.isControl && event.text === SHORTCUT_KEY && !event.isMeta && !event.isAlt && !event.isAutoRepeat) { + if (event.isAlt && event.text === SHORTCUT_KEY && !event.isControl && !event.isMeta && !event.isAutoRepeat) { toggleRecording(); } } From ce32f1704e6830dc7c533adf277067b444609b83 Mon Sep 17 00:00:00 2001 From: David Rowe Date: Sat, 24 Jun 2017 13:22:00 +1200 Subject: [PATCH 35/42] Start/cancel/finish recording logic --- scripts/developer/EZrecord.js | 85 +++++++++++++++++++++++++++++++---- 1 file changed, 77 insertions(+), 8 deletions(-) diff --git a/scripts/developer/EZrecord.js b/scripts/developer/EZrecord.js index bb173343df..f6ff15fa57 100644 --- a/scripts/developer/EZrecord.js +++ b/scripts/developer/EZrecord.js @@ -18,12 +18,79 @@ SHORTCUT_KEY = "r", // Alt modifier is assumed. tablet, button, - isRecording = false; + + Recorder; + + Recorder = (function () { + var IDLE = 0, + COUNTING_DOWN = 1, + RECORDING = 2, + recordingState = IDLE; + + function isRecording() { + return recordingState === COUNTING_DOWN || recordingState === RECORDING; + } + + function startRecording() { + + if (recordingState !== IDLE) { + return; + } + + // TODO + + recordingState = COUNTING_DOWN; + + } + + function cancelRecording() { + + // TODO + + recordingState = IDLE; + } + + function finishRecording() { + + // TODO + + recordingState = IDLE; + } + + function stopRecording() { + if (recordingState === COUNTING_DOWN) { + cancelRecording(); + } else { + finishRecording(); + } + } + + function setUp() { + + } + + function tearDown() { + if (recordingState !== IDLE) { + cancelRecording(); + } + } + + return { + isRecording: isRecording, + startRecording: startRecording, + stopRecording: stopRecording, + setUp: setUp, + tearDown: tearDown + }; + }()); function toggleRecording() { - isRecording = !isRecording; - button.editProperties({ isActive: isRecording }); - // TODO: Start/cancel/finish recording. + if (Recorder.isRecording()) { + Recorder.stopRecording(); + } else { + Recorder.startRecording(); + } + button.editProperties({ isActive: Recorder.isRecording() }); } function onKeyPressEvent(event) { @@ -42,6 +109,8 @@ return; } + Recorder.setUp(); + // Tablet/toolbar button. button = tablet.addButton({ icon: APP_ICON_INACTIVE, @@ -57,9 +126,10 @@ } function tearDown() { - if (isRecording) { - // TODO: Cancel recording. - } + + Controller.keyPressEvent.disconnect(onKeyPressEvent); + + Recorder.tearDown(); if (!tablet) { return; @@ -73,7 +143,6 @@ tablet = null; - Controller.keyPressEvent.disconnect(onKeyPressEvent); } setUp(); From 6543b74b52a1ae18004e3d4b0be5f3947263c7dd Mon Sep 17 00:00:00 2001 From: David Rowe Date: Sat, 24 Jun 2017 14:26:01 +1200 Subject: [PATCH 36/42] Add beeps --- scripts/developer/EZrecord.js | 65 ++++++++++++++++-- .../system/assets/sounds/countdown-tick.wav | Bin 0 -> 9702 bytes .../system/assets/sounds/finish-recording.wav | Bin 0 -> 63128 bytes .../system/assets/sounds/start-recording.wav | Bin 0 -> 63128 bytes 4 files changed, 58 insertions(+), 7 deletions(-) create mode 100644 scripts/system/assets/sounds/countdown-tick.wav create mode 100644 scripts/system/assets/sounds/finish-recording.wav create mode 100644 scripts/system/assets/sounds/start-recording.wav diff --git a/scripts/developer/EZrecord.js b/scripts/developer/EZrecord.js index f6ff15fa57..593f9d5fbb 100644 --- a/scripts/developer/EZrecord.js +++ b/scripts/developer/EZrecord.js @@ -21,36 +21,85 @@ Recorder; + function log(message) { + print(APP_NAME + ": " + message); + } + Recorder = (function () { var IDLE = 0, COUNTING_DOWN = 1, RECORDING = 2, - recordingState = IDLE; + recordingState = IDLE, + + countdownTimer, + countdownSeconds, + COUNTDOWN_SECONDS = 3, + + tickSound, + startRecordingSound, + finishRecordingSound, + TICK_SOUND = "../system/assets/sounds/countdown-tick.wav", + START_RECORDING_SOUND = "../system/assets/sounds/start-recording.wav", + FINISH_RECORDING_SOUND = "../system/assets/sounds/finish-recording.wav", + START_RECORDING_SOUND_DURATION = 1200, + SOUND_VOLUME = 0.2; + + function playSound(sound) { + Audio.playSound(sound, { + position: MyAvatar.position, + localOnly: true, + volume: SOUND_VOLUME + }); + } function isRecording() { return recordingState === COUNTING_DOWN || recordingState === RECORDING; } function startRecording() { - if (recordingState !== IDLE) { return; } - // TODO + log("Start countdown"); + countdownSeconds = COUNTDOWN_SECONDS; + playSound(tickSound); + countdownTimer = Script.setInterval(function () { + countdownSeconds -= 1; + if (countdownSeconds <= 0) { + Script.clearInterval(countdownTimer); + playSound(startRecordingSound); + log("Start recording"); + Script.setTimeout(function () { + // Delay start so that start beep is not included in recorded sound. + + // TODO + + }, START_RECORDING_SOUND_DURATION); + recordingState = RECORDING; + } else { + playSound(tickSound); + } + }, 1000); recordingState = COUNTING_DOWN; - } function cancelRecording() { + log("Cancel recording"); + if (recordingState === COUNTING_DOWN) { + Script.clearInterval(countdownTimer); + } else { - // TODO + // TODO + } recordingState = IDLE; } function finishRecording() { + log("Finish recording"); + playSound(finishRecordingSound); // TODO @@ -60,13 +109,15 @@ function stopRecording() { if (recordingState === COUNTING_DOWN) { cancelRecording(); - } else { + } else if (recordingState === RECORDING) { finishRecording(); } } function setUp() { - + tickSound = SoundCache.getSound(Script.resolvePath(TICK_SOUND)); + startRecordingSound = SoundCache.getSound(Script.resolvePath(START_RECORDING_SOUND)); + finishRecordingSound = SoundCache.getSound(Script.resolvePath(FINISH_RECORDING_SOUND)); } function tearDown() { diff --git a/scripts/system/assets/sounds/countdown-tick.wav b/scripts/system/assets/sounds/countdown-tick.wav new file mode 100644 index 0000000000000000000000000000000000000000..015e1f642e91b72578bca1c318f7a7c0d515d843 GIT binary patch literal 9702 zcmZ{q=T=m0v##%|XEultF{6N}hzO!!Kru%Y6%`S5{aa(~vHq=p>mc@te6N@z=A5%A ziWoo)h=__Jf_BfU+GEacczwS$_9=8%-Op9mJu5e?Ti22U)U2#t`(OY4Ka*1d5D07b z1OIm?fsu;;^Pm6opJ#Faz=rZfoxvr#hPGon)}q8_@I)EvOuykBJ+&Kj+V;{RJ4bh| ziQ27)ou+b@7Vu(O@74Gfk(IGx&cQgOAcim2!jJR{)!AQkj4sOq*DRmhZ%yJcGQ}0? zGTLlAX`?OS**Y1cIO6w4I{X)W9~bQ~?XeTMP7T&dUFOpe8&A15k5*%wR=F}+$oa_7 z;Y`+zHax>^oW%hgz**d;XY$_r;lmptlPDjHP-a!UO;)-2GTlwk!4T@QR%)Qy!-2oTm$Ttya=@thEK4 zr|CRQjk@t3jktv~*pGuahdXG(2lRx0R3-{gYGqW7tyqP5mP6w$g?xOq*Zf$nx)ZY7 z9o38cKwt7F?V}`&#uS>POL>#*a+`E1&A}8KMM?N&9rOwh>>?l4-E@Mk=_78{FYIdy zkGC9~>sI+&ylN?WZ(ew@KAG~zwF0Sw0^%)~;hLlr8q1`99) z=@<&dH@w9Y+`uX9#bI2qyY7YG?)Cc4OY^h5LVvMWE;U@C%c&UCFxHYes-4t=dcEu& z_y6S`_bhWqE1hsFyz4bGm2eaTt5)lwGGMcxzp( zGQ_1zu3Mn1QOQ-Z)-9Ago@B!q@}1wKQEz%@V*9*%GNNfGiqm%s=%*Fg0`@hiosMwi}d~8X9Kd z)X-Y1r0uqbN^AyAw4pHij(7HiZrUmA<3s+r$lX|z*DgKmFj6z!EGcyxw3@cq3Yu%v zXdF^3N?)vn9$8{LaoHYFvwh;k#ZhhA-WFXXPf zpofB~(d+Jsym8+|+)$aoGq6BcdzG>6k#(_>$jsQp$Z+3Wx4xsqcG>|xr?5uKy9B+IqCCQI|iM0BU+%>6lwQ|H=lKbwJbnq{W&}hr1IkuEG*)G~-e{83A zxL0!DU6LcNR_gegJwhvewHOVyahOi?Y$a{A9azt$lJ7EQgk$|dAM6?3!8toX`|UK{ zLZiJyH-+12d9={ha+Ph1ZzrYO5DI^1YoHr;GQOR3&z_?lKjCPkX7Vg8g

>|!Po6E-f)-rJVeL4Tz_6we;Vd1n{WwdvwUOJb z8$=`YA5%l~k!$05i2CtN$8`*3IuhH7QIu_kSc;9Ii22!5{3I-R5(8!0q=>d$;5%zqjvZZn#Y3 znO5@W)C8GMDqWGc(T1cO(UXzAddSZ6J$+6esaFOv-IZ$%SIBa&I5sshR+16r&(iGb z^)el|f6;NfO!d}GpDo-@H%*IZ1yyJbm+N9K)GSU@$GzIlFZ3>7&_ldePx1|Gz*~HS zVkpvShRpZY_}jcHf2~`nc{GWJi+SDtJJ+Z;`Hb$PgE(imxyjn>2QgAD!xi|8+M>$d)S^2wSP|ff4k;PKLS(XYHkN}?JE}hqdv=1Q(&^!6&nGB_g z6q0~d#)*saEFHsO_#P0K-0)8Od)*;Dhr9F~?bbuiQjtZotdus`zhydqkro#c7k1;A zU80BfiaYfeC1Z?D;UZnen{_8`#8NBdY)Nww?WGQTK_PMR9z8+V(O|9IrQvo`F6P@R z-lkPtrVBY=Cb zrD9#~Dx}7h%VJk3S)2yXe)30sPWN!Z5>ulMl$aWuz%z6{uhDH>rE7Vi=J6yQ#?bHl zUZ3(UJv5q-2-{apRA9PY!s$o z4lT2d_z!HtGMZ!AHi`$f6PIw*cJm3nN{`WsukbP0#_@Db5SL%(l}bKm+K7O-tPM?c zN6)&0vX9T$O=`4v)UDxmdNX4s-a6UNl~|+m-3*z)Lrw6F{>b#;?W7mhj$U|_W?3l2 z5|qd#=er+V47gl`$+(0v`A;V3>i+uKm7NRXR%xEEDqRyKC8Fr znZ2hU@l20%I(FDrT8;V0#RMJfM8DD-H0U*VQfqO9FZ%Z)Fa3|)hlqt!6A%}6*(O?U zb9kzb=4AQhIyFID*low@Djr!2f6*wVU>tHV7b~$Pp6OaZSu|2Z;^OCehtFHc>3YLG zmACvIrbFFCoq>{oxUikp+5+U!ADONXp64kkhp9R z#q=}3wtBjXI{nKXlS};2UQwt1a>@Q^Z>m2hvOKoQ+a=|^%w|)_>DGr&^vdqjB|Cyz zI$>AovAxDu@+}49X<%xkj7u#anMkEXrsEFI(LvisXY3{#>Aig?*f5%8d00s6Y&%uj zYFc2qaT35cdqYp`I-SA*acMjC!n2V)S!dA_lw&6<5E2)S)ntxZCt9dpFLO9GxI*>X z>^{qH9%N%^nibIsRA`M??l0y-B#6t}=>^@>^K_W@%1Q5r-{9U_S3J`#mlx=2uC(p8 zmKRE%H%W#u?7O|=M!o6I`1>LUV`sfP@{B(OnQkK})ACVD>um>ZwUwBM>A~CSeEGty z_QCpn^FBR@ z^L$sExLte5VI;EfzcSqhAr^2t9`RKr;V#3U z=`E6VZijBgYD(Ng=2I8m*kihmlh|X2>7w1Em((G>F3fby{&(WCB=_6@&oR-)unrtJ$4WxA37LpHi)HrujkG?FYNEuWL&(oE(71dV8rJB!w5LbLV zbswM6o7|}HxLb_|u4voNm9p9`&|IX`P!kHbQ?I*Ix`)CmnqJ_e{bWx^(qt6cVh)K* zmvga9^~U~tJ7yocva>do@UlbCTzH*)Ni<50Mrc#kE zbDL!+Z^Tk9bRluYx0CMM1v;X&?xb9E4bsYgI4-Q_ZL-6ymqlD)8JsG_JqWKmI~!y= zpOKrcQ3puC&D0XCqbl1*YivHvun8E-DqZ{*;nc_;cUaE5d-9w=qSqXyX_hOL#awPR zRAI}hI3O-drl@sNi$3Hlc8vbAV|2wHQVTxYZwfOV#kM@)^uLMA9EF)~cX5FZ>mGMX z!Yi7;nP4cTTP`iI)u==jtj+L{y6)fvrw7|< zG5)HrIYC@})GpEkdr1l6qA4~T%lQ9$56Ka)HdYt8=09?+^2PK06mPtr0!{g6ocM;;Fsm@A?m8$J=!cl_1a3Ig#n~6p2jd^LEFYT)W@n zoTXYOhweqH-4L z2=&=V8gMUmqMx`}WU!kc)7^aihkKzBAnT?!Yow6Vb-z37--1Oy zp@p1IK=_27apJN)NN~C)9G9OJDU5|?lqz_Igq)5+9F@WqFuH!vh0N@Aav>C8zm-?WePPi=XLx_d~$pb!Q7vMmuz?TP^cASI4^{z8~p| zwR#P5jZa!Fj?hKD$1m+8`Vd~xG+UR@M%tyD+;V@8H&sUSA5&vTX@JvdAk(#&R$_}) zyAA%oO^w~bc{-$f`Lx}@(_m^SB(7j;Xsw3Z$)S6Rn^Ar1p2`5HQ%GFagOIqafEVj} zszGQ*gYBfSx4>(w4~Q$WJ9gBC+o_-2Z$C7nUXd*0&9)O812Y=jLmE1+elv#m>Esa$Fz718C9Vwp-~X^{QKXSAR`AOZas9j7buFwzq3^rD(fp-&Go-NLCs zAy4*4M!e|Hq>p}LYHUy7)2)HuYL`Yl^ZlZHOK4{Ka^Kqr~k`~_-QU%6HXvVK`o?g zj53dZ%O}_0!qoCfy~YjF>H=GlVqZEXw; zv{~TO0}`+`*iPGYm7DKR_a^v*BNqLZ^e*}&670Z!J;WE}t{3ku$fZh#OYozskmase zr_xwUh96`N!WYVR^Kre*_1a89Z!wTHyeYn&Tqs#yTFga$`5o?+hSweMm6PsRh2e8@z#Y`Hbcf;wI^@T==cOv!f>kunazb07ffLXM zzM&_1uN|>OKV*IIkcN;SUAbQqzlVYpl!!~TFm{=aqgLy@Yq3X>R{sn87`X0iGgVs| zm7+j1T&j@k)pooHS{OPUzdNZB@9jGyXkj$G?x-?g6j~s;E?tKJ?hj+fhc%pMYo}f# zPL&KUh-Zye1jI!-I+jz^=Pqrfu!Z40dX&Q6B6vGFB57W>U+9)fIfn+?&6dd=*oxow ziJI}yF3~ZorIWah2E0L+`YuJsQvcPF=q-ZnB*IS%U#Osk30lRtf!1z@4aMKaZX2z( zdB~*+Hki!51^no4$|>H5@a^4_BdM|;VB)cYb~N6LW{sFN$4FwmOh z!j3e|nrhl$r8G-3d8Bbb0_gYY`|K1Y-cGW9_oA!ZHmP)LWdY@d{SfGfzUT0|)BSwN zLYJr?_@{wJV1U!(WE<~?g0>59EHO2DghB&t9sDca!l00s$OgAlHu5se(QG$bB5@~x zdb*;=0t1a}NH{JUBx54eqD4t7qMMVdqh(2@(fmlJj37qHkGO{mI2^Q!c9R?Bo!9M~ z8>$m%1{PR2HM~aVdoyAaB18XdVeB7MLo6d)rsh*A)}z`&$3@c+^h3UV;Z}L%u34S# zcE|NH>bXTaT~s1&v}V&BEVGTY6Mx)86n3O>_dD&*5OaA4)I^Fq|f79xv3jR7DB0#dsbp;(hbqxF>oYr)aN*zq7t{ zpQMjH8;Qv@OG{nAk0EhkDvhzEpoO8=TF+PQICfLQK-*`ES?C0)$bx3Qn!+yH&-8*W zx{o?E(X3-1PRFN416whe|9+t=DeU}dB8Hjq4|>7m5z)Id@l^&`v)M7{M75cBI(B ziA;CnWRMg2%5Us3u7}O~F}oD>Llls0#L_I$tou8?jk3groj*nF7lj?EUE*VYZRBL^ zn%AJM=pqUDQKxxxV=KHZT7@!Rq=~yzdc5{nQ{=9H&K=VIfvpG_g@35gFib=qO0X8& zF2fTB!+g_8lQ4bQvF6i!pOy^ZH*G<=PG#F}KoFD<)Ly3OK zI_OtC(=9Z1R>K3GKPLsASDQIBcCy<$;a~M0`>)+siMc^Kh7#8us0 z|LF#NIt^?mcLO$rL-*3Zof_VqG}Mf`?Hz^2PWJI(JCA#TfetdAQZ0iDY!TLTwQP1P zq{vN^F+AvRPWS7)T0Lf$z4~Z#(&yN3FG)rPR|hS%a_quJN?g$#39b$_(?h$2WAQH9 zf9$=HuOt*^I?a_8-WCZ-0Qty3a1ZskcE8C>Oij>5TO+qgw+DZyH&GJbz4Vhn@TCZa zEe!g?KsuialR_`F76VO zZ8S|qkuBrRvcqlAQkrF%LFXU%^q_?aoB#$gJ!ln!Oh+!}(`wyH+i9&XaCv^Z7ZO+h zcItk2NY3$HYqAggQ^6x-l9%r+c4K8K7u#}FU5|EfEowNw}1PV3}o(2-iU1;(zNYjguI(OEoMN0LXsw1Y$9!ZGZ_ zDcryld&A!X;);!nOqaQ|ind6l8_0Au>Y)$%%-xZ*bchbwS=^x}ZF4*v zR!Lj1%I4B^jJK4)y~H^^G)YJ4lHHHDw-z+(89Y23gUrlK(yq!p zky&fYe*Zsux~Cay=aUcBlG!t6{GXoyE2b@3}s`)d#!6$MD zE9DFx&H@4FZA1O1eUCD4dlx=x{&9~?*0B%VPFc{wh_d2yN_ zABMRB+#TCPbEpqaYVaGplP7eKui|+)4*Ov{tdsS$gZJYJxFFZ?zC45X_+1L%v4OOJ zj@Xk2xDj!@E06POxm3Ucnh__^NFLyN#tyD!Y(N=A`3CRld8o>6gbR3*4#e%a0oU<2?-3{HhP zvY7q}i?Z2;$=R4tMt!9Vw&BK9R|J0JXMTl`@HStjGkh5Lz!q37n`k#5!qa#;-t>>c zOZQ2BfSS%}Xsm!5iJeG#VzOEa$rDn07r%>&`gxBr~-=V8= z7LLJQ*+Oe&1MGx@a1t)TO}S4m;(Pxi1}MQs(vmuHPZ|_QxJf(%7s3izM)Tuzotz&K zy1Ne2TKlnX8k5%plU!aqG0C&O$SxehwuN$)kurs%!#$4hgHqq{Qh)&BTxP=ep1%8sBC{TwRxz^kX z`(RlZl}#?p%oc?eZYj^fX)sQPVt?ok?Vy>|Ly~Iz3UB2x-i51j7LUO`*d}XmBkYs| zbTVF`8}LA$!w2{Q5i@B3t)LV1!a+QOCQ3Ojl;v2#^Qc_L!wA%Gt^>B<`sg9ZS9&K; z)iIZkB;+sx-R$N8NTDMR1gOZu!XdT9xwoh%XlcK`C*w~5f{)* znSdjCAoY@t&GR%f0@DDD5Ir^_AL$I%Op|;!v>jBDd{0y(* zG28+BR|Ea#*1#s&Nr&P|cUf-A1A56H(7rin46UKF^rFE$iYD_+TEr`G8O(!eFb;>s zey%&W!{%7Zh&Aw4-q2INOIPDrcZ~PaHeQPxWCtF^lktMTQK-tEhxhaYqIhg5EwH2X z)XBMtvD__)6|N%8%gPJm{Rkd_J)r}%5Stu)$M^gctKhnvcPGMrw}aQ=dfXoO`Qzc7 zzZUMfXZ)7FaSb^xr54;Cd%yq~4&!AS&5z6cio$|yW|-(k#euFDcf?lMC}y6+cmE+i zrw4KaF5xLY;C6-$er?=Bd*z5u4)4licq5;wn$eZUrqoWlVm}#z<76t%g{86*7t1Wv zZ>~&wV;55Gl}JNrMV+}94VIBG31;FVUO~%bE=-5v%oxfCF$sF1Qk5@3QZq;4*0-EvRGcK?7lgOu!j3AC^PX#>;$@9?47jNWYk|4m6h5)P;Lv8H|=mG>aEen%q-y9l}z=M3kU5Yo{gZP|2$ai2Wi49$=*wOWh1KlW|h%;pY zu7C=eN7LgtH(dHd59lDx^_z2r@YTNyPkmLIoMFKAxL&r~H+SA$5BL1D@Xmeb0(#v? z+@5;!033l6csk9OWw0EIlN$)VsAFtF4aH-W@AyF{M>paHO8dyZIo|7z#j|u(?!r@i zD_@`n2uiWJw1e)@ABMp=oF;Q&8Ls4oGz%v4C^wjUL1%0&ji3%0l87(pq1;465^*PO zlGU&U_P`N5%U9@*Jd#)LbKzGB*3`|KhBmG%_rW1BR;JJ#UQGY+Vw%H~DW4p+g{Dv! z0Di+~c#V(cHeA6oeAw;b&9DYH@h*2bo}$ZQ|7!9PexXPSHkMY{S--h7xwz1+m?wvYIYe^PU&?$zICOOA!Tv<=tB4Q^*V;7;-dx)JZw3wcjJI4F`p z{pNboARd7eafW_#vP|Z=@-W_yi2bRDw1*bhfSn7`kOWlWb=G~f&u>nth_MKSMYLJNHfC(KT`Km>`1MoA!G?jxEJw0-Aove zcJc;V%Uft4AEk3#Nw$wbzd8DYBy@5x4d&ufUg;Kvnc3u;Q6*(Hz3X(TY2_Mo2@3ua z`{w8tU!v3YueJ%+;1=19NBA^dk=yuKUcqPhjUaWPDYR7#2t#BvO^LJJlDNt(;n_G< z#^4a?LtVKIHidcu^gDiXulW%glE}ZgHIn|-PRk{{B@ghW7?O|@XpF6;6ZV!tIGQK9 zS#cq)l%+5ar^#3truzunb2BQ9=xf3k_g0?pU4JzglE7`S4h%`aNzutwx##>Ie?Wvx z8c<8_K)rY%j)aLY0~c@wR=|8HhY2tO20~B$=CA>H2=HCr(KEh>*WrR3cL&0Dzdo*` z?Ys|7*O8QH>m*!^`QlHPz7y>AEWbpmG1Eqd@l!cR5-^*l z#L;9J5ZXv%tSk1->Ez@VUg6X3FzB~l4D#8%P~ zd&6KEiIXH4`VX@||F6de@gE54H_cn_}0IXo`=aJwja-;M|PxVyl{ zN#J|=Mu8nRfR@rh_YsZY@mQ{t!{t05XTU@lY2RE&XbI_GtvI}L zUO7T%;}v%&KBCv~MSc^;x~?g=p{}v79~#EEsWMwR30{P=aY`KRhlD<^tF(qD#eIaY z)cXtM`$Nq)BmxTNszSia5_rkK@RY z1nR$N$6!uJL7u4E^KxC_$Z!(tBFlg3@AvY+zi`8cj<>| zAI*bhuo4#GES}^>$3fIvI$>*WM0JGWCw}A?WSj(_Dq=v|!n^T^oWV-CBaiU4e4^j1 z=)DQGja_`-Q0B(U6q$`n;2&6mb9gcuC*iurHrSNvMsdIS6Ta4eHAU}m1QY|Vk&TH> z(tQLuCqWWRT;tfvcMiRsO%DG;63R(X|Jcp7i%nf=L{}5_K6>o#hDv`f9QFI+R$9v& zXeS>aLlS(S(&Qp!&@fHT4GJUNL@5_V@5^AmA_+Hw`$G@RFIy?#H+hTCpo*{g3x(s^ z{__N_K?SU<_nl|abnw+A`d=npn z?IUI~HYs+Z-aN>Sj+1DXEQFP~Oy=r0moT9It8qO?Tn)-5J)wJ2iRbtz?TcIey11Tp z$^kh+7x_BfmuCqBlCnuHp(FOAIROx4OJVKZq9-7rTIZf;yccG=1#MH1LYI-~BR;^bcO zLw7q~a;M`V+AW)KHE*Jvli*u=AHiq*%|dmhF}9J;iUE~P!r8pYtqP0%9G(JWaVYo2 zZrGNaP(22$#xHC!B=7*|ljCRZefZ{M^w>}*2R-30B%%9A zc|f$u*=1WFyiQKu(Q~PS>v$m^_XlFyN84o|99NFqIEi=4NeE$mX@>2kI}U*1G7hKf zeMBqb0zb1bF&kAFl=Y4s6DL8;Kj@=87e((E@e~-6z_qZI_xPjX3{`@061tBNp`J9Q zcAVIxu`pHU(9*clEsnF?6dsLb&__D!Ws72k!v2yIh4bz@ zSK)Jb58ol^eWYv>=bQu>l28m7%d?3!BeH>^hwCUUq=Cx{q2znbyX<+aq8oexPr?D% z0j6%aeeo!rhiiP-J&kYNSFS-PcG+Tg=r6-)JWqppxJ*{yB4v|gq;llYnOet&u7soR zBY8m&;|+Hyo^-}Zz#87-_lAT4bsy!Ele*B9+J>&aUmSvCX=<45m&BESa}v@S+F%o| zL%_e>=lIe+3b*`~c-kG|S~lr#BvH#rG=VnI1^Qr_GOaR8^qX7a=Z2|noD7k^)Q#I> zGbja8^iFT$V}CbR;yF1&h9trUr~Ak-U{W{Sj~HFfCM6^RlM*K(%W%GZbHh+4*PdGF zHwOjy2Jhe*R7v6i_p@>2MO`Xgw(^d?$Aa@*A6sCDG&!0e({VmjaD`hC%gNLY?&&&) z7Oo*=9O#F|#NBj6&OoJ3PE6g!GR}-BHo|xWd$I zZdl?BN!0G6PS6?~D@V>h@smzYbtyiDhhcZ(BoqU}8Mp#DlAzx#SQioxn5!FTNJ19l z9MOF=MEXG2w2vfblWIBgBeI*e#MN$-?39CansW?DALWNCimGm)apXM8O^*3K!g(?k z$EAIQ?WGx%3Q`S!ac{#DcTXziEFI&VlSqo9auQrc&)L`{%Gfvw>;*PCn!)pNIaKgG zUmnNf2pA|m$P`7@rQkcfhi6Hb!V_>nwyQ3M+m$1?%eE>$_3z?WD(HPA&0UAk-46)E z+<2a*JRq*%1#V`X=tl8CG)_VqVhOPEfY0#(+?0!WN>ouaT?+I*I>(iG7oNZy$az53 zrKBtMlcA8S8@P%W;VhX%qh+wF8_-6-xx`7(D|uMdrIaIw)w0>`DI6|2Q*+ttK6*yG5#REj~YuW=!}X1N5zRnHYsruFbs{8kY-q42y5gEyv3(-7xl6| zCVO#Px@;*&5_lidJ~H%3Z37URfqE=&dEBw|oM~2>$PeS%3MG;Jwf(vj1 z?&EWQ=e|c(HyY^VT+cYbjo|Uh14;!hm-#qDCMX7kp4d@Tm&#O^l6Sg~;09cT6Mu6O za2(I`Rd+8wA>||rLR3FA%i0&ZW&P7P2lHrISW&X5W=1wC)T$e7-yG!XhGIZE#RtiJ z60lYFxTE2WuMD^8F}{hPeRTj=FE(*)ix^Nj^0?Toii_NAy=uNhk*7KRyYY9AzP7KML=? z`6ObFB-AH?iA_?q{VqP0H~5vS9mRUe1CntPG8U%cJXs1!mzpf2U@-KC&bp7dgv8MM z3wVgP_>wyv4*FeTW45MdbIG2)jYzx&TUkk6@6S*UmbS@tC`@$BtmN%d|T;u}XI@pf;<+x4`s_+@Vb>HlptFOA0 z>k<3gH&>4Hm6M1IXogIHk*FB3Q*5calmz!9zIQM9KHR{IazZa#^GVp`Sam7&NxbDR zScAm%s2R7V?%a=Ta;9#qphYrMIdUFMeYg{~mPVqJ`$?bp1sW&ePKQH&ci8CHgw1|W zIPBFYafct#YyL#P^P38*W2FkYMkTx$tNNGaX60W=`f!v;&Q(*%y5%rgbbu!Y`TyGB<`3N4_el8!#4z zN?+<4+xVuTuE)afk}ox{N*)((yGl7@|7y8U0!-bIoAi)hxR3D%GL~>7YAv0hHxA~J zI7!jFTN#$Rc{~lr<1p?=X>z6L6urwEc#8Mv%$EvfTB)23142*b$fZ7cE}&vSy^qYB#QSN7tcP`AlT%JYRBeCjz7=XpTuo`# zJhm%3TwuD)rDa83ibv95?nRxrB{foA%I+i6rQ9Vx35Rf(DvDxiyCMl?lN19cP9k}e z)F)w@?Oa}xd=jGj$k?QG**b{&&Ap^Yatp4&8RY@p=I|f4nRnq~MelNp9`Q^1#J@22 zNf;+Fgh%5aBr&ImBvjqdZ%)8f$ItXe%$t<6NqQe`f?aS(PSPd50Yyd8IC5_3I&&{H zhl~BItt=u58HNK?m&!4qsvB=r-QcVKeBoHh{_3qI>%w~40SDwb6cxpHx{t(BLpDwV z)!{OpW~fghR5;s5$^%mFxO7=8xbN|ud(KsOLoVd#eFLk*C1;c1o;)G_S1YKZXlzn9 z>d(V)yx2a%m9h|OeG=Tsw=OiyN(wRi89tEuBocZ*h`VGXsHXki**Wz9OG`^v)P{Wwe zJ9d|T=98Gtb7`5ZkcDnmoJ6B!kRplXlK{hj_>v#sExeqgchjY0Hyw>@j-MfU zlbWb+5Bnystjwk*yb6~jhYJjmKG;>-M;q$Yf!H}Hh-xU2Cj9h1GV71#0x zw=*2bP8Kfss zZ<0AK!@lf9;k>^d?@_I~ktS#Nk-40C9#+5#SSaN*F^=$qLQm=ht)u~FV(JFI;QMp~ z3`y_~+6Zf9EAEq{cup$mZhY(vNvKy=IdVl3xh|#m(MmKVp^i)H6FWKeCe_h>r0Rxw zlP=3CyN@=aDvEpLFrA5)-5q{}ukmyIWty!iirh#4)!Y>00axJ?niHn@vBA_0bzG8h z66$hxkNGw#(|VZp@Mctp%T7APrf#^0@dbXwU##C;BWw-Ik(Z%OE-dsb!_u4w9GZN4 z>Xmisa{e0M;8VQIm1OFMY=w2OQFhS5B1aydXCJD+*Nwk@orZ-Lt`qmfKb%BD@8#;- z!+~fH7igdyIe(*f>bR`kN85QFt(R@EACL1nmroA9>3x(GMZ_FrN`@6AG6-;Mj~xXf~k zlfy+havI8g)!{;R+467tBCp^v-r*~h)b=fLwciwWxhnQ;CbT#b4~&esKW(sz+!rh!TWIsY=CuQ-XuCNrr8$N4XAHE@*c52 zog9 z8L7IFdu3%JspHbTNyW=ny|RC0H;j{rSKWD5$K_UaTqcfO&gVR!Aqntcx|Gx+3F85m z!vdIr6V)p#J)k3{>_*^k?me4tj}1xicE6#pwq#rN-a5yt&(*nBbJsn^x8X}x2!v^J zngAHi<7hh1#bvNk7CLh|%OL4Row*e@!V->(0qJFY;BOW#X0_Q3@Aq_NZ?kYDgQR}?SP892hb6$ARsyi1V;T!!25 z2y%6U>cqyrP3Y|UaE>I@;X+H}96v3L^+SVA&NmD79LaC_?B0aO?k`RvB>>3&>V_(KE>4H>GK>aDcWIw1iUs~^|7!Q(8d&N^w&D7s zR~9Z9l6YQ35~{W<4=4k01dsRSg?ZWXP(ce|22b!K3j?#Bg$`NEU}`)3!1wYD?<+?R zC-I=VoWazMc+{PX#wNM9@r$d8=<3I2)E>ICWjAR0ztj!WrBrP~szg&a-_+%-x|F4E2FP$SHc2^hSP04k=KILC;s&UBsqgp!UL#fwclojLfHk!VfNtuQO$h*5lGr3SIgX+-RTLAOi>QguUmDU#qnK8ZOnP1Oyj4wv@oa0%$m;UZ7qZc&$# zt!`b|;CF@t?xb7*seBXOdX!vb8v%bfWn>|d=rcHrh@J_-2F zZ^?WTkaH4lJsX<@$K@Pd=X>}J-ifIj)PS2~2kA}&ST$SYBx)~PL+{uLT0tWyfym$8 zhxpt*VD;_gY!a-MEx0#fKvg&7HGYw#C^kjQZon|sywn`Glr_6yilRDPs7q|^^VE%A zw(e25=`Y7qco>t~Zb$;p@MU*9JoL3l!Zh2i$>EYXi6Wb%`$#znMG~>DGbF*U)a6W- zd`8`0vYFPzjg()u@g}J^>7)EWmh3OD>?oNCvuIIRku3{zX}XM4uPk<#cDj$yNi}@M zxBSH2WyS4c75`y^r7|<_;Hlk3S@ccMW3;>Igm6;UW_fM-IzjKAKNLvxM9M zTY$No`5P*dxChtiLOkvc@OIds-XyZg;WfFNdy{IwnNOmFbho_JIGP^j`ii*RE#w(6 z2}j8wb-1Wc0#X7XeRFo%!Xeo~8{-QO_Moj zsT-KhlilbznEF5$Zta@HI%Mb_UTFfrU13A-yvePO>XSH3XTTgT%1PAbrPSfl2ZpHQ zl4f(dY>kt!$#vtlu4$<2P=3P~e8Z2`<*cb2%@X2TbvY|1fr%r}CkG|m(6tJkvfhP3 z*{GU{B{ORll&lPv-OzpH2E=aE9-7BeZ&^Zk8=v@l;VPZS<8hzgR#=;DC~Wr!See%A z#slWrjo84ME~WcO#^VfFAQhUr;c_=o(Yy3e-=1s89!*h{XQZi{i<-KD9bEM79fR|l z-N2`is~b8wY9G6~{?Qag^UAKkMUcEn>Xq#bt)-!+K+P+w9J#3*a!_`_i!)v8Ou6T6Sh;i8jM-yWEv z7;n%6cp>lMr+#y~k65!CgEYGVrfy(`%*E+4E+Gl*Ak8$t3kCdU|7v&TsyiQ!`TcPl zu7_G(%G8Z#^j^L}P~RT4kPgrj2l8+?Ax_8nxSUPhpu|b^z>e6G8<6=V)a5KyiIaeX zxI=vsvK98hG36vQy8&-=b)%FlyMg^>D2`J$36`;?K;2|b-RM42$0gPw4nMMwH7~M< zd^2$pypz=7vYGbq5lw+6wH-beCx7H|MJR~$MJ4>=P_Sk|N1m_F!E557s z?H$McT)U4{-6%RP={wgV3FrX>aYP)SmDkKGsR+w8FO}2{=?R_EQkhmo@32!8 z1L}Qb4i{yUbRTiGr$Rm7jLmVW-yBTi97$+?S1()5Zb)lt97-|?KPja^Z^C6f&4=9X zxQSM$$yw^=7MROfemRJBNY(aSmqPPNq~x`wZicGEh1!NDS-qgG1g0p`9loL*xn<^a zpM;#G%l|?WCD52!r+wr{{qf53TsJ+MH%XBMYIcL--|B|3Nht+NJNN*ego~=S!*hA3 zc`0?cV9F9&@;a9L1);((SM<)~X@m^WeUvbu`Xul@KcmD0p2&TB>-ld^qL`O*QcBG= zzl#HuBR6%ULYAvn7AC-IjiGRZqS2R z>u`Zaph>r0n5RHtp|VNpl}*_V>K@xU^U7Lwqn4A1$7nxn+)k!$;0>;F&tskd zP_L}ymu*rv+;YzQB;1IY_epRA$S6ePB+_L|C!!{=*W)@+&GxvQ!@N%dzex?6x?xBn zp?6Dx(n?-H=5PsveDBc7Y0|9(nSbC%d>QX&Hwzd2sdx}~!6y7$m%5|66n^2~WRA;( z-u1HODQ-?&qCN?lEt9pCKzhU9eG;|0l-@_h{4VUq!+eSiNvPVcSwgPknuOL_mzv&n z%Bn|YQ{pUKtm+2Lg=wlNYFB_X+-hj1U?wa_MO66z5A?UT^#2G^<^a2QYX zC3h=4$X*pbx?jRlLXD-BbS7hyl#@WifHYU8ivFwh{o`FjmL-&Pr0CsJpkbRo5RUr` zockn-shd1YI1uvuE-#}MGT(SW+eg$Xd6QJ#us(?@xIq`g34bu`@Eh5Xgr-0>0iX^S z_^N#pSSn35FXj5P?IW3!ys~K@Q5h#5urbupRsv04D<^@Px;d;Ekn<)GJOh<-n{0BX zZq!ZLjf5o7*d!=AF2~T2XsH`&3iVi=r9h+YBhqg!_etukD|<#%hF zKy)9u@i8T@SD>Xp6DOhjNV9|#<9Gide3Du=|4QdMX7@UKmPvVREBwQ&s)yp>V zfNolzC0v*sm-%JeN;wI=Y*{(-2kut1{I2=-*5DT2?T-{rXIBchvd7_-`<&EvX^d^5 ztMqY03ZwlLnPo}0#kFdC;sKjVUD5s0G%J1sGnzBi} z5LeZvdXtos zfaP4_=NG1D6GG0Bqcwp@LA|p0Jf&{%gzU#1vL4ri@_@FFT@$^7*H=; zZ6$yTHirvMf>AsuHGxP&D$#$n0qs!52*cxI6Jmx@^mfwZ$n%&4biLd;Ip2}V2$np3euPh#r<9JayiCmWos!MSTOsSg@ zn!GONce%XQo1_kx24s_ych0gKnwL^eLLDx+tvIf) zLTle7)LNeKX{%oWA0agE;?cBrDL4i|bL&*gpUaN&m3iaTO2931n>MMDxY%~>;5 z>`ooH8Pyk(8kZvpOl!S->!-$)0@cj?6rG%1w$=o~Lgw4c z(Yx&3iV4|p~ioUh9unec#bT2 zoo5LT#N(vN>*U+h_MKeaP)?$#Zh-nEtWylEl|VXDOI0@%N#F;5PWQw0>|!|K4$2PM zfa_$d_DN`dSMC%$Tr|HMo4WRF7!b$tR6jQ?)$9h$Li0(eq8P1D0)&1k1_Vppz-c+8 z=$%*lEn&Ai5>F?G3%=HVBNnvOjix}k49BQ0l^ibCOl7GX_3b(3$RYls&te=oRKgkU z6mwg`YQHJ&G#=2JKr97nsT*jV_ZwMWs@P10bK}%(T+L9QvV_fIJ-uxG*YJj(!d*>) zf;wEb;X2;ncj`V07yOM_rA;96-NoR&a^#()Ck(=oY}pOX?`jIvP2k}$!1W02U5i-X zc`mqbg}2$WaL-+r^K!g4FO~Wv5_*41wvQn3fYwaK!&H|_*$vAQPRK^a+;PbbQQLP+ z-5|YeFY*a@FdCAet&$@NxQp5+k>{my-(Ekoyp-m5ah2HQ5+}i(sWmrpb(rxNYj#81 zcM=BNbUerQUZXr$1iWDij7@cbzE{8Bu*lElV+=LPdN$oCP@=lF9KEPE@x{3 zp`+?eQm?G$ci|M6E(I^;1O1Fq9WK@ems$xXYYH^3aLb~q8)H*;1KR)1Nob#hyB5y+ zW1J?p-t9;{Al-nJ0{yNjP->W(5@n!_a1%p$wjfmCa>anoyt1K(?--j?1F&W)O#o0; zxaKd!<7CZLab32(W?y!k&shreskXs6OWi#Ms@H^uMTB~Y19L2L->F*XEXK!jgm#B}tG#Uo9PO-5?hYM(@*c2Eo>T>Rk zZA`Oev=pfJNx&64ttrsB$*pGf?U}>1hqf!%yHYFxb!o0Go**0_aBdKpsFI&Zc#mhFX zLsK{8xVuoemQ{sk?mg?|oN2Zlp{L|XBFK zDW=&CenZMhXpdiWw(JJvURhr1R>p;H7Egq{<5F7*v<)ulQkDX>4j1Lfd9|~4(wOT~ z_*!b!4eK{DhfAE|=I~)F)xRvVS;f2cK$o!!TeeHq*&z zc0+x8Zf>+*+2q@k!O*+dPHNc=^GS#$0NmlQ({19_v_*DQco{v6KRJi{c7entT%4PAc7?UFwxB zRA-7Lo+Kopmo0DgYYQ8)o#6nbW-4t2Hcle7lWM0}u1l4Z^~&OWD3|dxBHHA9i(p6s zzR|n*)K&2{P^LBSa3O0a73-A^PyM^_H7gXHZxEYf2ej;lwv*y~UgrMQPMWeC+9{Un zQV+1UQ%p8!pM*97t5;SXF76F~0ZZMuX53b~Qhy#wVy~<PCLblj!W*LURjtZf37(>w6yx zF7+E(uPhd`gqGhmhs(8i*FB4G-M7?A0J$zzRNG~lvPpR>f#sz_%dAl$^UUAny?;@- z?{D%&I7J8J4re|IR^OgYPCG7bA88|SGj5C41VZC@D$Z4(MCufS(OAa4U6-J0yS2e7 z2IPl$TiZ$1<*a=YvPpKkBf(NP_y}L6yp*aN+Df2#snjc*+DUmfSbo=<5|cM6S2zB6 zWy#t})#dCC#grv9hl}nby^mrmQg6~=-A6n_du6GXlfeGmjoNcFtPc*YoizC*ysGWm zzO#n?Q;#=h}ACn;h2X zW~BF#b+{BAm+F<(wrp)m#Lk$j8}b9xM{bHDp5lXUSKNq(Bv3J+HYGmh*ZxcR?J+9T znj;DI?cp3n?{Sfz9W6_Uc^jNMF8P;wlkgGTw$#lL%}dFD^8D_pzr3>Axoo@j574OKUwR3}GY# z-OMYionkV;^$Z<+%h-bzG{Vs2!J}sheY(-_JFGyvcPZp9AA0EfMC8o*pl+QbU2>!mkT#D>pAD2d6w`G zo1`4MA&DYKt{pC^Q%u`QlQ&5x=g!5WWIUivPStir@ArL?BQMbus5&l1TXM7!IPa67 za<1(Z>_VZCTm;(k1V+AC}OD1O7D!$sS&2ZA99-AA}0cR7!s zLDEw?x|X4V&tfen!PluayHVRKdzP;9o#aiT8b&O|W>I;-emG1y2{$(^%~sYdESXi9 zp7P>V!g78Bs4GO zOAA<1Q}ZQzn_SK*zgyUvtqtl;I;hQ5@w$7Uj!U-uu6BwgudFtK;EZCgZ1U~tH>ZwE z*MPN|>Z>+^=;X91v4|uzzstw84en1nY4RpuTy*qbI>odtJ2inM zj-0NkS61G_x6}jzEtE~t$!WilCa)#W@A60(q^TR%D)k#_8=Ou~IdUZKyAHib39U4xd!raJM;cgTvI6XsH{+fU0hMPU?o{ce%4F zipg;q7r9lcOXUsqc{?f8WAN4CvwI_tl2?|FvR<~UaTD%}2mL9%kK}>8ppTsLyA3rj zWexS3-<|nqax#vGC6}`z3GF$TF9`#}HPp83eMO&yJHW?Nepj2Rl#|e2+1%kWFb=o3 z1!^6aG(ms>4M&30fFu zxJf(`(>`iRmb$^8{DEHbeNt~yvEOJ7Y%O+NDw0STPrne=$XS&b3o4IWFBU zww`lU+x5j1r#xUuXqdGsbo9MrtxrOhgT0uNjV}!Is%~iePQrlJb58fuV#hHi3|OWy>`)|L4UN7#Mq$bgXVs(lB_8n0P?r$O|V@8(iv> zC?W}KsJC`fO@X5MBzP!|mucE@scqSAmcA{ZDbRG;N+!|$3?KZ`vj4fBkprhPp~$P2FhExwaC-McM3{$t7d*ro`G#F?+osZ>UfGMr^uNJmgP@ zOa4}TNU!)K7JYlQO&~BC(%S+{!yG@&++Tf@!$mm>tWk%HHdCqNl8?HSmn!Zf)1~5b z|Do``Bo>mQ*eSIVsJf9}OmXwWbT^)d!2s&P?M<_FQbXU^Ixgj!y)E#6?G!8OQn5Yd zF6Y!Ik=_=7iTS%59n^8@vJkxXNhC!vrEZGfoOxwaJ1M-?wrr$W&s!6SauPh2tmjn0@ttPxoAQfVJ)kDv9NYjz{{c4cvtpB!i6B3LQO`n|-h?b&3@om#GN^i@v@8)^pCrNmyI<0BxuTOMyZtw!y)5xBw^AE35k`Z%X`c zJ?DS5Wlum=6fL_UnshS^h^ceg*d)yos=9#};UpY{Vl&mg^wtfTx}m+YY<&{0opk-9 zZrHmU#!1lVIN0@(+?%A$RPs??*7nNUyBkG^3!UN1cpJ^(!oMiJ1Jwq*V$!9wm0+>^ zr?5Di6Q;PasSQpn1)7>boH<-X9hZl34{L|ZM$)Fl(@B@oo^$y5S3`X-7>uJ0z2}^S zc8Y;D)GLxuuWYP##*vFP0>d%&NobZ(+u-B`8j^tLn0L5v1J^QkaF)8ET-~4wnTO>( zUVVF%_neD^V0{wW<97`&&40GyBpdD zrw*6AE&H8(g+k)UrCrjcWSrB~&9bmOTbP=ujFZsJyy;T#lRj!*D&C+=aLW25HW+%} zs|f(8)Z}&Hb;*}P;v|}3JGO>;O@Ss1h&nk2e3RspXc;@`K2qPFOfOLe`eW@zh2GLcc9GSqRmv+ahboGrw$i+O7Gbk>Q$FYonm5_Eve&j`M(^Oc^h2r zxU|$wa$Lq8ZbMw_w((v#M(0AMw|3Ixl}+uW*5RV5o2i<*Nl3!(BYUODwGO7XQ{GO> zH{IoUs+ix!Ensi2r597+HR$UNHo3%+(-0n$QlQ!dl6uay2_%1aL*EuKBms}*HX55m zmb#IRvP&J8co}cS2hP-uB9Z`mF-3K$IMeA1Ui!AcAIGJ=m;!&jy{4UFd{plvF_&}V z$kEE2eG%`yZjiW)Q^V}6lSEqX^vl-vfNls9m(18ENugzf z1Y%rSxYzh2FkHYnM}ODcYszpj?}knigc%T|#IjAw=GV#bE>M0N)4L@u^?kI0y0$>obwjvIfhSS6N&IQrTkYuhZ)t?yYXtA zS69VO#|Rvf$vbS?Q|Kgo1{-p-D^4n>NZJ@ zDSm03AN0B*qvZ+zcvM<8coLn2dL%CKSA0N4Y%Hscam*<#I8 zIw@M9bOEG9bFXzWE=i3kAX8z}iYEs;sdWiK0)a6Fj7$G^J-eC~^)BU$F$G-N{9C8R z)pC2h1%Gqx{_ZbL^SZP^pR*Pym}1m!-58k));!#$vUWH8<*Z!U(g4s&f7faC*M#ANyS&Eo*ci%q^c7+CKJPGur zEGLmC@ECIoG+(y3D0-xv?b>c`0TAjzN>mygta-|~RJQ=KvP#QVu54_*GNRIsCg|T&8l}aMl1&8eBuB zV)zb=F@^67QIBHH8RrMVf_nLbO z*(9^LhGlszt>m|54qTv`{u-Lqn1c5aT~{LTxAl?gK)tVeH?DVTab@Kj!7X4ra&3XO z?^57NfCSRGKdoKtHa3|`GP$r=FooAW#IK2j>d!F)7)-*Kk8lCR615FhyNy&nvy zhNqFN3>RPgyR;ORBKUU+S=q<6>6X$z{-*qr&-#<`tLjkv0DL3f&}3y{@ZwA8%-dax z9?e6-$a-w`a-8Ngafv4fYaWa#={aZFB;}lYrIW&%Cx_j~={+P|%)N%|M&n*{_oGzZ zBRGD#O;Qh(a?VNnyYaPZV*R`yK|``7E<3kL$K8>#2mCLq#kI=}cvJ48dCh$p{#_(4 z+ZI>2Ky8w+oCFPM7%re?!{VBc@Bt)T)CF4ZQZT0EEBZI5{$1D`9?HtDkyYSHSPPU# zWzEXEV0OKFrv3i2jBdMLH!uT|cfHa8KRg$n1kXc%*K!hivwev*;gb8f zR!&a)QdyZ|blA2@jeCvNnwKxS-!=VJta+?tDiDE-HE($?{#_VT+~Cv~mLhfC7%5Rx z&c(a2&5S8@T=_=7aoHw$7`#Tr!a0`(RGQbu6q7NXoOA3XK!b}vh7$=1m%V0AHtz-> zYZxv_xcF9T(@B-NyZ}21So1)pGCHZK2cKUb0T!y6xOfmF#`5)12 zlEXG>RDOY8b+6|)(Yg`j_R0)sv~2v%VFoOWOTBL7_i2~I_mQTY(E>$o56x>ZF15cq z)IFOqy-x?bbTS-^hn&($VN4NCTyk;UfR>$zdoYN= z&)AOKb`qE*!omo9!}!kD4cu#Han-Es7fGAfB`d4hq;x7=sIRsD>bO^RW@TZNRL;3g z67KCZ+bf~oeDKr*ISE+Bk^aiZ!;#c>uYm|$ta)es)h=0CY+6Tq^})-0TFPrNy@PR? z)d!DkQtDax*x~-BKfrK#*y0L|rr9KAT%x}#Jh~cKHvHO|sUF!}pPrKx81U z8}t%S$*((|0UJ7Le%ZZ{(YjHKYtsTvXZ&|rt$EJgM>{=yhhL@d9J(8SG_9NJv8dOL zvQ0v6Pu*(?{#|&WhSNYU7FW7SzwlZ4o|+xGT@;N3@@+a@UGDh3b*I)Z+b;gy>2Z!< zEGuQLTbAGAH+X7#$-|u7o+QGb@v1*BKS<%6r-n{ij4A1Y@r^J^*q1GQ@cA`A$6TQB z!COuuYYWu(PhI-E^b?*OauRB9&`H8vpmaW83xC!3f~1z~ONDdZPF!~3ob$M{2gEiB zq(trSHjGPHip;&HR08!KHWt^01j5$ccq^-a7cNkl;k3V7534kBnY1reU8&E6@AFa1 z%DS>6SNhI(V2Zgv(j6a@_r38x0#5?wWVmk3y;gjOu;zi3*fyqM(`ru+*1X1;5)LGF zugOIv0xO-glXIS?1tTS{qh<1zdm}4P0_J2`^NI^p`@8l&Vl}47Hn%q^=Nu#uxNg8V zYB=XGciQXjk}TwrmfCER9;$t%rugCEJt zIR{g$Gyu$;tk(@Afxwzqtl~}c+PZ{Fg9BGquN(U0z?B6lF{?RQ?%{ppE?^HB^}3<7 z?6&Jhjgsa&6zcB!+TiD`|dWZd2DwnT-(ie_+I{y z4!dLNhw6OC)#}f9*DF&D-3`l0xLKkW*LH62WmsIXBlo?sV{rVGP!B2rb`oYx!6Z?p zcig3rlPC-q;~RkptTZ^CB+$H;$@C&<**p;RohrscCk3Hi87@0zli!Hx9q%J^ui?ox zJPFc-3m0Pw=%nVlan{{<(mfvgRIQlYJ^C~2b)(^&H`fjMM8A*zPxl&Hpl*(BlkjCL zZ^%?k@BZn=1qx$|(%?>o+(DGWkAHm#2!C*d^VBI@6@odjsvaNQ_ZRt;Xp z6vIx!^_Ricy3xDTqO8UgFvXm93DqsoPr4C-Z{&ZC=c^yLek@(7MTL zOhIn1Nw^p((JxHP+<$0gUgVLJDE-}H6@SR$N}r;;;gl!gK1_$q2cLB(f$1IoU07T( zNs#(?(Ym1}V7N4`8!1oDH!aZbtOZItm2ZTqP$W%Gux&h8P zb`oH?6xYpm+RJSk92x-bf$b#9bpu3Tv*uxvSfG}o!jk~UuS*M5Ip^*7QK7+=2EecU zSHl!D5(qfwg-THRyI@?pan|3poP@j6(BRMlrTttOF3xxomQ5<%4dwW?8JD1wcJd@Z zgG(d*@Him!&Ps!8EUx~mbilta@1nn}7FRWw~9NcToB!R4KLVs7?Yi>b8R(3o;kCZdK z8(=QyKb;clHQ`cr62-sU-br9bPQ=}%AFdmi0b!JEXmHq(bNjlXOfhuk|6;pKosciM z?cY^W;$b8%^SSC;$Ia?q%B=1+QH!g(1@yW>tK@ADBJj&`tXJXZo6fnhP zpI|4knRj!WbN*0Pb|5`TqovaakER<^&NKWg@ddBhB<`KP1LIOt&ajH>_t8$-=sK?( z-{&)QMQ6ZToD^dUNFZ*8%%de>ipe|l7SBqrRN9wH16k>$P3s2VN2xGe(t3yP#sU5^ zod~C+UN@R;QpxR^Iav)}vdk}tvqEXzV3IJ4tGPgtayI|2{;F*Up&nLo{HjTLa%S-2 zLa2v(%`MOWaVrvNaIf(s9;=q3e$>P3`zUK)icSdnyL;gl2wUsjt&chmRmW2C@5)`D z>#FLSm3`gAEwC=F3l>;5fy2PtQxWvdU~vPs1U@2Z zlVT5OJ96>HImeD%eei_@f-WJyju{YlDR9osm;%NncoN{8^9N~X*c>Ry924J){1&Tn4PA&iCJ?JPPwwixP&nU3>UnQU=_D* zlKVUz^Jsx;R#wz1&V%g9C3S)NWww*R4CrBRu<2dDZ2cX(D7tUck#HdH^E>i-*6W7) z;FV6AZ?HB1a8bnjh+j&tgx8h6Q#j{&hCMmP4A`4P=}TS7Ac3fRt&?$SeJP`5gL96Y z1YDr?$kq$>WUo94Wr}Hkx06iO>?FWBm$N0C^pJavn(l^PH$bLJ{oT;Uzl&=-t+bSL zQWq#a4Ia0uA_^6S*ha0&Nm5yvM=bVCLk{={SDX{BQBEV0Wp$JlKth z&sP)UR6jc};N|rH^762#p52<(F)@@$0<9apw*P8834bJ^1*$sGby8pQp$d*k0N*LG!! zQE7o14KAMY7c#Ex#lKsq1nq>2x@8Auo7*phH&`B*T z3l}KJRPtogy0M)EXmH+0AWEo5^ST(k+WuYjK<(g-v{~TaRri|pcg+Qg)(sLaW^rvu zAZba!j$9Ki^aKx--c)t*?;=CU$e-0}2z_$2OWt{Y}dXWF`sstCPW@W*+)bFF6NL+?@ z(>`|~A2F`1xj@O;MbUD5GR>OTcpqVs;I_pTI|+iNNV^+!Djy4n>oQ5u_PocbHP8JN z?Yf~miN*z*C$jo?k&~bmd4bnS0?cJnV+x~reJjHUe~OOdeFTfETQAC$mBY#uJBO?+ z2=#?Z0Jp#deR5!2Dh&?T_B1JsizA#iuhF{6x4E5_ZHy^q@Ja{$*ZCx$qpMlnYreuZ z$@;slZE+=S0LToQ%$jiF$Fln1MH82}w!5$L0oR@+@_u(PA8p=8{I~iJd#3?GCy9>9 z)y$~YJiXa!^V)XgG7#20WM$DMl(U^X2_u2<{`{#|tGM;0jB(kODTXd#Qih8wKKO_H zyZ7v(h#Am*E+5f*V7Oo>;Xun)YhJS>2Uj)>iBJ16GC|tL6tdjjYv9WI5pl5ZFWp7_ z{rH#vEnk2KDi>>BAycWP2rf`Cm*YJz2`T5t-BW2aS~ngHm!+=IN!|23iN_Hbmyg;` zqI3yC-%-xFgTZS{)~syd8(ozf{%(w+ioL@?{hKq_&71BWBwWCig$q2RQ`O-5k*ZtN)(!ni7p!^B+TDPA z4J}Z(K>v460`4`M2%ZGKY(c1(g>Ft#GL=^v94xLNCB|Q?i}jD;ME-(4<$bh0{kPg$ z@5Zkh+-nadE;ZrelmudyBI8N$yyh;|^mom@meiPHG&oqr-R88@>oy5wDkLu9-<2S} zEw`ui9VOKB8eWqZJN(VPVqdn%%A$4SEGKb>zo&BDaGQ+;A_w@Wb~ns-*x`KY83vR+ z;N&!&=aCZXu}wn4g~!{IBQ`lj z|EZT%3*#HkQqE7OL9R_FZQe&p1TOtu)|~_&&!@a~H~vL;11`{1IOp;P$W(cqTWQT} zSjAb5DXBX%m+nSGCzZW^M_TW^QA!{tlkKG;@#l=S!Y0ZhSVxAyvVwfVC9@=-APdAm#s2f+`EO8_#J8U8Vvw;#&Chbi_rot&1+f)=CWmz zl4ynpLE?xfZ-C`1}~ljLcMO2{vWNNg}83GiSoQW?T5fE zkc8C*>MrI&gTqdOb&|lBt=RXGP7<9ycx&C1E}{B&wRHoY#E7ol4f&0=bpyVU>;s{` zb0>kmlx1bHX|?8cH9EfNUZq)$#g#xxoRKt}gq?)zjm~^lV@gswY51((U+s(=-6qNOn=|n_FJeJ>~eFZ@e3wcd3RchJ;J}%ikC2wC3se z(H!I?Fav4>;I%veoGg^Pfi@*_E-fx} zQl;;7GA4nZJSinKiC7>45+_qT`aEY%kX_Xg2fegsq#L0(6m62{xUjgUgV5SRT7uh-M}^pEztO9 z08c_w&h&9ZC(USH>rMg*m-bz%Z*!M|e>W|FHIG(+C*h{0iQxEU^}#!)2U%x8`pIeH zGVh`7ZhbBWuVN{>%74NG1sYs>+$))ir=(eaL0;OpKxLYLIgNGdUgKWqOZ`u#n0?vO zE-+ls-!+3*QaY)**YdMwPDW0G*3cSWBJ*gbUEA|;dW_qXgj4q#eZ|EGFB|>NxZi&! zU+S0bW%ozE84dlAeNP7rIWTvAjQ2FcGP>TPvaM^GvNZG zq?(iI88kO{?w!OY4R&<+e4?eDDHua>KaPoJ3>r!X%N^;Dt%TV|rhli!p`O;)afxjb-be5q=0Ub2r~9@CMC-aDH+*0@U{aarzC zg-THRQZRV&RGR^19k0|)>uWTL$L0}!XzK5J3JIq7-@>H?#^sUpN!S;aaml;oLsz;R z`Im4r-VHfc&X{7O!4$I@(63Fa!s2>vH6y<4UVsM*^c`yRB=}4+8r=I42Cq%Li!>{% z1}~i?D3+Xr_IJ?&6|_KCyJnm8YI-rWnPPB(f(BPyH~2m>bMg+iKYbd$jNe*!Bi#-i zk$o?D6b3IDE7-KQIp_H;|3-YZ8efmDo~j=YeRB(UseIFGP9iIzzA<>gz1DO$oaQ8w za?ZgwO3(XA50ReIknrt43_sh%LAUVUrS#{3O!;!@FY4L(!4S$H!0}!jt#CkMs@2` zY(lIKfCO6hdRF=lsq(i+x^OmVZ*f`Eqf@`P>DlrW)wu6nv21lNtHq`W;e z(&$@Ia=={9>*1H8;utl*l)s@P!U$LQ7MBU}{e2X`)B4S!~QcNH%oZnplFI3v6n7l$=TO_gX~$8lj~YG?;= zq%}{@sBPHS;mLu(c+?s33eAF=bjtQ-lvGL%Be!RkqS7vjqdM+-K;_r_-?JF<8zNdUW*bHl*X}FxK{Uw1`*8T3X7unv_%-+NG`~g}3J} zSca%j+cwaSyf=JWe^H%CKf(-_ZutjUSpPQEy8(N{ELKX6aw=xaR6h|G5AJqgV7F}UrV{hYO2JJ+*m705w#upTG0(ICKS*P%nHFwHJ+SYc0cFuobrt{ z{e>B@m@56b_)EB%?z$`<_an8f7T%r%wQaTX)2Fj;aMQ5DsYvpV-0)ZHpIaw8zG@wc znntG0mMR0Go`22_%emNun#&)XO=z`E@9<0ILOYNkIsDskv)i?fbN&>4qi=lV_BQ4n zykECT#W4!{jtF+-@Ib}qTN68`R&(M)zoHNr-+Y7)ly%^2Dfq2G#;q^^e%76u)}tt9U#e>GwgOC zB_fRsH?_P;OKELkXHIi3+wQ&4cWVBd{s3!*tR1tTck(9W)8zv>Xw3p#6w^OGW$h9g zwT&k?%;mfauAA132QRl?h$H>*jHa8Hh+b2}+at}9$IaEI`d!#zVJpvP{FQiJiB~T5 zaF6O6-@F^fe^r(T6zIpm0;mdu|*4=8V3hsj`NhDY`pQ z7igLgUW;$|g|b}M`epT7)f*9asnNbzA(-d^a^%>wB17aqjeElO_+O-)9j5maaExjm z@V*FaLPnA|axX35WwI7~K%N5vBisUH96yo=Ajx}k(zL&ibb2ovT;s~3T?+nd;{br0 z8t)@olUJ%8cDhU|=47LAt0$L!!IM+Ik-m@Yy9?91W@W8in$TaxM*ptITyx~f!yW9g9pDDUDeW~$U*H8oYrbxk=lNQr5PeUK=X{)h#MK1!9U|@7mM&pw$1J6FcyeUFYU-CNdBB>? z;B^|AR{t3cUU>tuN$5+-F?TAdTL4^HS4nTtDH|mDG}F%q|5a%Rb8Yu-`Sj2$vWu*n z*7UPGi6;jSRNe%JOPfY;f(n z3!eGh;kDO)T=BW3jx=~Tz&BC{fO-i*;eO~AtSo1AH~IZV&C4_-e?2e318ojv^IH5m?ULB29ngU%h?VmPrD{tGw5L8ovnO$~IOA(qA>5B=9~0Zx1{P zn*88(bq^HKxi; zXrvkc6}&x=67_zxg?EF%NXO_0)_gkOOQ{MSy(;kbo|PBOCIm{3Ij>BclYTK9 zNnU=z_YoCxZ`cA;WqRLfK3&e>y9>8qa_|y%?(38!zp8Owr&s(-*uA4OpMsZskJU?v zhBRzK4(TtjGuwVCx#X^=+rA@-*GWRuOGxErt5^I5)-LtWCF@JI;DM6A#Co>Jb7zix zyX@uSm%{sqZp(c~rIGd|x`fCO>CJXsTox8)b&MiahE51~&*E{jy^g;*kP=HfW<76{ z_xNKuteyD_`5OJ@{!JN|Kc@`?8syP7My0qLcO0&IjDwGK9dL%(Z+BNqh-H~grX)C=a zIs;;Zi@Ox-ldEyF1x2*&CCsb+s#NlTwg*J2jGVP&H#3#AQ$E?}nxk0fyu0FdVy;d5!+6X{yXRiu?#ysm3rS1)xVl=ogD8W`vMv|wCq+LR=zr0QoNuL}piteKP zO1Joa76r*&HE^nCs!zvrQ89OS#f? z!}n2{-j%gt+blczgWM)CBAeu5 zbXBkVQ9q1E!nPWxhk5C3w+dgjG@obY#xXiD!&I5DO_Hlh5yjt}=Eyr)&hSgYnx{!l zRu2@?$RJZS)Hd~bIAu9Y+f<23!o#+DA)NxXjnuZ9)`uN&Z}>EUcA(DdG6TYHH^@CL z&&hZ{wVqWiNU%33$4^~1y1}J>nkwt)%#oXykaoBYaci}w{wRE&)Ku9xucgxih3_LZ z9LF`)%6gHXOUiQAEB>RYhr{2T_L}4jeXq1^%Mfkm-K5+k`T-0Vx+&Pbx4&$$O-c=G z#rA+R?0rPTX#n>Uhe<+vO>H-|HqtD~*=9LolHe#z`0g5Nmp?x}&2b=8X|n*gU;;l~ zIeremYWI?ETFd*Wa|T2VwUoQj^#rP{iL^mA6jar5Nd?hN|Q*RD9PTpwMz|W!x*sqDx)1X%+QQ($72uZ7Nn(q zU0&`MpBX2<`#A~wv1V&J}C3D%4i|ql`lPjy;%Djl@ zvgQHPGsO<;vQpce{>|a?r-tK=nBM8be9(U#b&`M;!l&p$uQ(tW-)M54?&qb&yxKbR zdY95|k_?buKEwoCwo`ipD5A{_h|awGEFP`&7F_yLF882EjoK#a0MPW8a{SUv|7v;> z=47X)%C26*GQID~Tb1Fm@1dsKZPq7;orFA9T%fbbSSyy~RE{4%oy(JhmoWbZG8MOz zC|vwf*1U#^#U=^t$gz{~&&8L*l>EBPSA*9IT9jwIX*9|5fT4ft?l^ZiH5`N5rthPD z?%lK@Y>m79M{)=|3AhEkb>=(MU%HbBnj?=Z1Bk%W<0Nm6QE>bmYlcX!=&u@R*|tqW z^SZO6SW;z;lO{jo)GCfiBCj-4CALZQqPA|3zwj>SyZU{k4e8>^-Hg9E5EzkiHcmyp z!}?{*k5c=(!Ei9jXL+@A9G@<)D8+*)m@amD|{e;H3yYO1W! zbOV{HuWM{VQyH}EWo&MNLg6l^%5GV_rjfx~X&eA>Q}Z@ZPrXvO3C(M>mABBd%}!LA!i zaNS_#CdtcgoOR|kpRTlQ+-zaD!z2Nl5Eb7cng#AGQqEwwINJm2`$#6a8F6m4IIi?+ zs>ulAM-b6IA)zB(q5#T1;1*Ac*T@jRo{A0csf1j%5@_z^Z!Z9+(Me2r}~Le zX$QFC6Hy2L=Dgi(!GCR@oOR~gwsQ4LwMb_`7>>c)E36fK+0MZv;R?%HO)Tw6f|cAW z)@|S1CSIN7bMgym{sNPP43J@ARL2XgSLzu-scl*BQf5x3XRQ;$KE>*8eWUuN{y8Wq zF@6ws=S{TP?TYV*gZU`6J0X}PxF_rlxNg7!V7xt6zZ9}bQrMZur{_%fSZBcWL-?jX zTz!;w^H$m5-c9?$0bKFz$$Nf*iTOj%`vp z;nYhgJ7@!KLmn_6)`ZIixhCbhA-yrZ$h0acQ3;ILwBpI}P<|9mw^VcK63Tat z1Hf-erFjk70rKet0%Ni#r+29r++>>p{c5nB^E?o*Tti^&&0ZpUO`0QDFCpGXd9yNH zK48tK%Ne@De}LtzGoZ1*#?s3UGazh2Ytb&XFI&QSmSWl7RD?;TRq4`h#|mA2jbH_pPV{An))S;e_cre%IY3|8HYsV-jf(>SJ=q^3&IR2iSAYx!ol7gNdY4Oe2vt1{Egk4vgG5!od0)3sSZ!)Snd=IfFp|Czpb zU&YU=58_U@nKnzIaMMY5HeK;Iv*sl9E;STgLdy`TO{mRHez-ZWyLQ{{E|qq> zEwDn^j-0;(KfTa*n0loC)zErGg>BL_nayaVHO+!4xG0kPJP7xrf3xPuNi#&bJULh) z8kTdu$iMULbl+3%#+^9{og|PW$8`h5tA@U#Ea&FQA=7$-F}?50+j#?R6YYe+EkGCC zujwyb6hV@Q;n+P#GHdLEqsarRipdV@z$6(mD<}yg~&2_`=ikqs<^-kPuPv?Z~ANudJgeyQ2oPY!8hIl*7%Phb=B8-eV+5VlcJKagRujpXjA9mIo!y>-a$BVkfst}%y-V4b zZSLm#!zQF2s4_{&T3#l$lNg`6<|Nvb9CsoYN)9Fo-L&c+P`d0S@*`nT{bcoAe929v zH^e+p%XtymMG=nCA!2#J2du0WI>TKXX0teFTQN`+P+NQGUs*m zeEn1y6dwuQqErv+Tj&zvF6EEJPy9aFCja%@mk)4q8hfpKZ}hVSSoHdolp=~2s4YYlC+^+4g37KOwP4|=YSflSKp*|r;PQSnbkiX(Xq&A_nDeQ{x z`-2V3*_ziny{ntrjgyzbxWo(ybFwI>!oQedD<7D9aX>?w{&dJms8t--jl88HFe2fC zUp35tm?Y4%^+VDKUlNzfBVP;WH9GV7RSQqn0|QKz_C7LNwq7@659=fW`cC|!b>hKK ztxNGYcbgt$@!gVclR75}blYW_<@S(GvaBqcZn#T@-|LIvbjEd~u{S7z(S4D=F*6t@ ziCThYWz!QL#EvQQI=w|p-CDQ>;_OiFQZ$qwEz^730pO2XLz*`6maNSJ{rtIHlx{ZhGYswB0oTFRNe=0oX&usd!Fo1;2L;kv=}9&W_HeKg*l zDBlR|FW7|IS=onn5?qm>+m1#W8MVz)&c%66?Yk7LdBlv1qEd3g#Ml^)*TKIF$EbtP zqvRy;WedY`;{d>2iY#%t&*(e$tCny12MJHkc;wHc*Mw`kwqw+J4c{TdEihR6Qg=sc zw<|_TeA&`|+3q*Qtzl2ZUFsOJN!FQ1ZjT-TEgPgnd><8qmu6-C^fVE6SlG%LMBqBz z34f%^?q~Ww9Sxtw{b6U?=(fl%`2fuV_k)~mTOpK^Q?Bik-Aqv1cy(Uom!!F__&i`K zYVK0DlR(0S4)KS1m)nvzqNA8U^Iyg9s-Nr2;YKPu2{>t3?G0^`{A&MBY+E716Y;Uo zCkC(lSDW7H8~!{aC!twcvqIz(u$6cHX+MN@Fv;p zG~uGZYSsxsRu=CgO}K!yl5n$K%&U1dn%A2C8pmyppU+Y44Wgc0IpjaAcU4>B#;`N) z51+-OzAJslv3A?zh-z%dq}FunH8~_M^)BUJN~8RfV*K<>|2m_#{opL+tUC#0Wo?p> zpZulxd$^Mxs0Rw3+|q5&Z@7iB3|U#D?=YHfYGNVHN%-6FZpe=yC8FDoZIW!0JzggX zHL?8aZ^eHD#ma+mX{5v{G^>yj^~=@>jOZw)e!fSfs-xp}>w3JzXIOcA_T&l;PTQrH zaLISQq`}%N9oMtW?K@MAotpnLjxdfg%!g5Qusb99Ywuv#3%f7xfqT$N8V*8 zAtT%naQtkN=p-dtw;el)@Lv64>u~+G*Sl14UWcqy0&J7`l{~{~Ru*5jATauuqdqx5 zAonECF_&&Tc;ro&uzcAb%3t}D>1Y2-f~m4h66kL5IJ+q3w`mocZlE22Q(=iq?I@NV zIiJrbUAar)SFNFN%eVeaycCR6p(J@xn-Dxu=-JW=5EzRY3=II~RHQ0Ug5mgAIq!ay zZ|Do%v~J}Myd!B&f=>9;@nZNr-J%DMa?ACTLDt=X*+jh?%5u)wv}(cyJ`WH7?gP2y zIunSZrZet4>$c~2X_MOm z+Ce@<$L!iJH~F6EHc1B1a5pNxVEk8E%&U==b%mSMTsPWZw%V8KbO3){PeK!y`7O6hbb2?IGmim39g_r8dJyg;?Y5hj(CwG)vfgit?^Pei!|og9r#GGX z3U?_?5@_AzGD(2tj4#`!qZlo5S(EN0U=yORA9|6;Dn%6`I?_-yA=c z9!HZ@8*vk7fM>&AbM+#as&?k{xasWSE#E4dfBJ@vM);J0Zmwn+&@ zVBIDOva(9L#AS`drFjX-TA*{(lMB~PnIy7WA-E4$ptfmN_PjeK$Kz1j*1PSg zOcKpbLg2ivobrwAy5VM(J)pjim?m>72Wk zZ@POrz4zgPX?QiN%dJKQ1@s#B_*0C7v2rMOZm;R3D*tl z%p-A0H{IP7VU&cI@PF8ZRyTerxNhPwbT`C@{NQf4KRbSDovSS?YhJ>_O%m+L&$vQ6 zz$6j-hoP~2+0q$;9T}D)wL;J( z`HeM4Zr2U;Z1V)xNy079Yu)mPva*AdT@=+Q*>v08r?QuJ(8m9pymP|tn5Rk%KW!W*|G9p^D}N-R!R=MgtSJMl@#q~-Q_n`}UL!&;#APvKH(?<8X1V02IMMouDs z=nuCW05{Nth~;xYS>@UgiL>oJ28rX;zkA2dBb~%ENge_2DX|a4#04r27V0S?#uOk{!I? z`cl{?;mZ~Ym()GZY}8w|!Y!3#Xgf$|txn>8UUnEl}$c{x5Rx1|%+#li+WR!d=_~ z!IeouUiA2~Map@NET^}XE8D(H;bv?7U9>>4X)Udr&9s$Z%{%1O=TUMJP4jvfx*OO@ z(3^R&Ulmt|_D+JHCFIC^+6<^!*>kWrfJ)$Y=Plexy!u|wxGU+8@K3B=X#nJBSSJZI z0CJOVMb{%G&!#{$wThVkaRV(H`EGH~M#z=Jv3YkV|wOISDubvhw!a7@DXz z+eNZWpPbU*;!D+Y^^<-Ofxbggvq^jrJaRf>_6FWita(T1gupSXU$(wINzii*DUp`Z z+RnRF{d{Xg$KaqWXX?PUJzYXyN>k3*No?fp*hw_>9W<}iFcPFn3( zAaN?E)U zWZg-mfgX-gfs+O#`P}rS_M~mTG_Q@DlrDn8%^ggso9~wg@d(!bZv94UVaL*HZG6Xr zojFx=5>KVaLtkxP-{;$KQ>Pz2wn+_-d`H_Yp#5E3H*jr-n;J$*^mlodyruT<sjf#_Igbza-w99Yg7xjlVyrN8@{%<*rh`33PtIG)S!Y0W3AJ?-H760?No(BlTuhaD3_SC$f3~db-%6%3H}z+--+^kq zjo(WjhQqPTPQu~)s83Rx5c<2Y3Bhj1D|vC68)l?QG|r7oL;V2hDGrQF*cMYPr*RGO3f&F9i~cUF10X5;i?;9EEGd%PG<#}gh|+5O6*+)jI8s)S$4T}bMe@{)po zB)8v3?)5w$w$;46TGTP8HN9hEH8ze2x|4_;>ZsQZYk@jsWx*lBMe##(j2g#Jtq>uD zlsL>iOD|+~0KnOa&OCviu3xr1oF22B1iozXtJd^)Ww+a!H@V&UeLARjsp7om2;LsL zgqS4wsA+U5>wd z>k^v5YXpsv3GOw~YdgNXaBX)}k|r+QAbAAc4K;ZEmM(a-K)>Km&?UqS*!<1u4EQVG zD%qrBIL4E6Z{+z%qAo?squkTQ{!Psz>FX#%}MQ)GgB zA&=q_G@J%Wf9@mQ*h}VG?$T|(DZkPczL3wjQ}Qhxr;~J=&hurtMt|_F{I`FQq9Y2q zo9~_axq&i>hs#s^tc>QdGK!y(CwQm~0Z6{?p&czwuSNEa&Mg zUy#f0TKe7Hlw16d+;bf%2841d?w)(PM{-{tzytFjH!Kf#!(V>iobx$p5he=&SC#^C9}Ozr5~ zG3SgK6DDM>S=FOzy`aCn|F_(}Ydo`_$jZt^i~hf#0JZbiF8sg#pZ_x?8vqax%C`Xg zzjX)%+3>&r*Z=un|MyS*J9nl&G8o3fRGi0^w1!vV5}eDEaV!j$Uf2ahzTta$g16)X z9S^(RCf~*D|iu<;3OG|`I3#99MM;K4Uh1; zG{6zu$?M&YaEQ)AV|b8y6@B(m@R*GQWCTu>SyE0lQU}YhoM&JWjDS4pMn1H2Aqduup8=e8y}!Eas}>VlYE5V;IX^(!x1tbW@5Qi!#b>yg*clhxRIehWkW`2 zOMQu6`G?^eHsE32P8)a!9)WXkT^`aK{(>=NLQlwtQ7}nnQw7&x9ae`jH-igtIP`;V zLf8r)&vh?Fq-+MZSqo~ipOgQMahYhp~j`BHoBRq0%!&etVrpw`c91X=%f{S6LtkvHY zrc+@ULb=iv5PymOu6OY=pK$xbX4))!-~?WR+wv4Xzz+oJf_-=}jgu)jkC(VL@v2m1 zJcp*pXc&aOr4ulI=eO=jxQQ3&ShPEH6xjgO=e&&grjDeG|uA982IHF^lI zDO>teL6|_Zr5tK-oh*ZLmb%XzF}? z!#&|Q_zhSxsTU8#(J)y`X)&+FwYZei=!JcyD<)8cu)Dy*SZyqM;2F^%DY)QdY2ON+dvCwzl0$Whph8+j`ol+$n( z@AFIgB(3138|CqEEW(*#fnOHaQ4K7D*)qY6jQhK6PGJOJ>??27uN-#3df16a_$*zQ zhwui!N(7nMQwHEDnFu9R5o%mrSV{|II*ylN*iW+1@h|r=yl`psf~~kwcEfSFfVboc zyubrO`{K;8+;Ky_S7Y_>q4bzETkV$_L_T znS`ZM0V}aqs-O&~%Qze+eIZNHi$BnFzC)M!1nuX|xEc1s3AyNQ$4}h{{;udneRwdA zgQ+qXt6&YRhDw=(Q)Dy^=3dxY7{9|icq%vP0w1M4To2pmKse>E#&_Mz@X@t0VR!5o zhPfh{&SidCRO?spLM)MqI8p{+Pf1}cU+8ssNY|mkzVd&=PCN_^(nt@(EBXqNq+HLq zzZ=ODX?9qcTG3XQu`DWcGi5vshdeO!f=}=g?!#3)%?D_k)WdE(Cg<^{JjQqO4H-I1 zFB~MJaf*~;rL2OrSjBT`8l=(7LHI!*V?dYXnxw{;0({pI%9}YuT%7vjk&P|W! zrIto({i;wwrCjVrhkWWKnGo=syurtE1JBD5+$|e$yBvTsc#ZE-6Mv%L?63#q!3dZD zGoc)o!@sab^i`V3BV2#(fhqZoU%1IV2-n=%aFBM$df0_WqycW=BY6!i7;^^Y*jGLY zN}vK)N-b8)e4H+YFbs3q&W9R(+%6Tz#Ycj4&Uo zp-xs%1<$79Fw*BoJu@<+$hF98ewe!6){t=|+Tk|Hb~ub@`5HZxCisGpc*w>9Ji<*3 zv)sb4oYslmUT#7dPI=NDJp6{w(1iE-syh=M$k-NbaJ%IwoQIoe=!MK3=mm46f>wn! zZfTh3(zlnQm;AtHd>(GQ%i)CEC!1lj?4=WY5%1vh@ZSFnfL)<4425wp4dz1?uH}_f z$)z+|-)PXUJis@6Cy((aT#%!*JJkDa@d0<5ui$-nDIe)~aMT_0WH=Rt8B{LSxK5Vi zBA!JPxqt>p56F-<>_9I$L_742=EKy$jr53L(N~TrgR?OoMuPpWQiF9+EoC$#6uRM@ z3*E@Yzx+pj0r&8VoPq`$%U5`A+Y{Q@RZo`MBR3Z=@}-Ury3xzJo8I8GZpl7UaS(D3obb7OH%0yn+@( zsZ7Grk`FnUDKWP2YkJJr-MMgtcFB6!j)&w7T*C+44^h*lq(J;v=~ z3$uXaCpOD-x*IOJQ(>Rm!kcif9M{_mp1}u6qgNR0$3;_8^P(kwO;|;hG6$yM7#>8u zxf6+eQ}lvcctO8%*a%zk0G#G4bWdKuCu)t!cjsIjF5_VaETCmv3oBp|mcS$_zLbAGXtBIV;zkqSqH`gAD8`{c)sB#M!bCS4bT$g9S2!io!6L zC*6eTH-Ds;a!;<1p%>TVZZPzMC-T;Pi$bb%oZ|-ZXq*CbWHGLmHBg1~WGar6!O$1G ziqOySo}S5VyaXp?Z~N^fm++1}gJ$>%h*{jn4UNb7X<;5Ml{K&m7fT6F=20|Ia-b6i zZlSmG2yVi8IBE=IyD^Nba37lBvoZ`Y^pYadH+p$o=W0T^o6QrbAmq91;9VPk##eG5 z8|4fhf*rCRcfk>9pc~<#d&6HbmJH010W>O1awTDrTPd}&l*)KIj)!5!FpyfgnO?{p zxlE^cA8qB0Zf|_dUktbCsl1c#PC{qbCk&#oG6m;KC9J_UQVDZmDvX7}&__CxD8t|< zs0`yc?%|EJHSG7N;>-SC{M>!yUr3Tg{dg#i57VhkmQo$BzzUfS#WX4m@HtUNDvrO> z8+c@2`C7}KB-hGJjtN_Kn6Gq?kn z;Uw&XEwCB(!3nq|x9J(br=Kj;mHXgO7$;NR{CJ658&!DYBB&y`^iN!J9uaJtNg`8qz9H`oF}GBHPtIYKE{_?6Mx zjHS_hnvR8GsLSQ9Nb-}K!*h3+FTp9=A2$0<@g8@AFVJm%N*}^^4{;aYCm!s^@f4cJ zOK=UWmc?#vJlT!WHyS#tK0@#K3EkrJcpUafy)lf_a7FIoOKGOoK)zd?OT)1!O!o_- z>Qr5*!9`Fa6KNz5z@A{}#jo%YT$i(Sgm=(-eWRfP8u6jL!UVmtr9X^ReFPUt4Xndu zQigVWVIFrQC#`?qUT#}l?{|lzctLM3cn9AkP-n@3K`;g;!yKrDRj?MSU_MNfu{4DH zP*)aIedL+krc2?t-xoIF7TJp@;j-MO=i!6<$&jE|80QSVSQ$ozF^tiyUwJ18(n4?f z5#5w?${g94qnvivqI(%l(I@wt9rfUTZbV$7Q$>$<~RVdAqA1%Ui6T! z(OEu7J9s_r!Xu%<--sTj-h{6dONR8Md>%=~G8^=!t_w?3Wzlp$J`AOPk_7~<_>rF* z!#E|%FgD2^JT4c|ZZG^Hj9s7)42Cp%(HdTjOJojA#j!HT-e{5U@E)JaExZWFd5_x^ zwwOM01@G~5`pCbCIE`LB9m}8^>Qo<**{(PqnaYoQVkXA?)x8N1@dliOBe+Y}%XT`% zXQ)x`OA~$J-<0AWG=N9oM4TPUeN9~Fmh%FfsSE@1pa(nuJNlGrito`iISmJKJ8z)f ze1y)+4S1~R1pzy84h@u1_PZ{IRk&8{cf~>((vDuPnLE%6^^M+#CrtNxiXY%70O;*C zL~k#g7nY>fwynyjj7t6Fc(fZNInv2-+~VHJW4H+y@EGr+jj|OE;%UC(?uQq~9G&W3 zFkI0K7jQML;~HFqvsEAA{**1r@A}F;jIa5IaENv&!+;~u0N3FWyvFqHMfqGnlXy1i zS6&CpU;)g)A|6Kl^eg9I^hsXGU3Vou8a@>jGdu342Cg~MlVdDSjyvuPraKtnHSqc7Zq51~=c(BZJ%Z-{oJjzkS9L$BD-3v+mYAJsM~ zqa7HDMQ+U91uibQ9F2F6-z396z7<%dNs?1SV zyEUPb=CbM|gP{+oYHzm}-R28;9QMK{*lKU|6}dw%xEX&Tg)E;N4|RrKvJ~qieS5(u z%;%miGepz^Z}_3=BX|UqVeC+bL5<TtO%z?m>XhErbX z?mV~BXI1Utnw+MCybbGNHyo97bWQ&4?Gq!ab#K{O^z_H*OKZZ+HOg;QlL4w2r_6##zV2YyPo z+wNpMk3W#17v$nF8Hdw(zFQivO|6V7d})|Oqa_~=z4)s!4D~Qn_o_G5Ucd4O&?KLz zEjZUBcexywgQ1tqqzPe!>mRz4>LZ_}2_C>TIGdms@1!((;T!o10W+03!YG*p zrC1>=sFtg+Or~KW41-+A5~N?@gMSgT9Bm^_TqtVOel79!eXc1RF!#CV`(V&!9?Btz|ZgwT+**x zHo48w-prG2m)sqBCLi3-7<^XT#|`CiI1T1Y6)D45%%w1yMu&l}SIBf6THITH3^(a~ zIO=xu2HXY*IMFR1y~Xm2oXqV_BH4s=e#SSw#E`Rrk6}s(T&4t-O)+jlL*1 zP51iFOkKDa4wf;-FqX(_WsW#krodP<=J@yRCC6~Ds3NeRPr5672VUSu{^=-WQLYTd z@iGm|cq!HL3Re-&_QjFiUM`a(Xu&t~5DmR#CvFhaM-uen&)9~k(8J#7iKt(Bjeg}+ z&NFcWjF3EKj__O2i|)(S1ih$Uc7f?$@EG563kB7^Bp;MH8hSx3E`|9rO}#0X8@f8= zpV$olfnMrO@oo1kR^7{(BMp(UGS$tCm-w||6;{F=m;z&95Gcc7_zv&nG2II1{jsnc zH{!PVK*s6licjBO=1t|vFf5W8$?YXIut?DhM&ba;h75>M-{^*=DV}7l-7i2;X4%Dw4OJ$L% z_DfZ@=R!9u?(4Gx(l7a-x|du+)kn7QM%v5A>7v|%XYgLW6G&(E9NpMhb+1Zq&N|GM z$@-P6vn~wZ)mc~Fi;vMBsfVp}AU>74(snoVWm~gvWrA+lPt2Qw1o_>Cx>AtuIKIUa5(E4!vlJ)-jt-2IpRo}0JCusFL&!gHI>T@D8dok zkGg~BR`)4>>F&c-KJ5<1TV1`~Ug!A+K7n_NUeH-`FwGogF`8;G^F%$zA+B%eLa3f2 zJ&$jvE=4E&KHh|zl{xZ7cPBh^AHokp>58iErf)A4ZPDbu|^p)$wr+>b`^c$&!zXgSxZ=cwpK1);y{UKGI>X~Ku1uhL=J4jX7^eAu50 zjijpmSBMjD3Pzza3{!V`sVj>+m?M(>Qq^AW;uSdw2Vg5~GKOK!I=<&`DA2_i##ot5 zbGZ^%!y2h{^WrIPtbOHa-AkUxExO3ZlwtU-@jiEoFXLTJOvq27kmYiDD2~^!T$W<3 z-d?8ej!M*Bj$KQ5O^@WdG`J)2&eVo@I~|fU*r@sld{KrWJ+MC%fZblQocg~njXejrkEP+4Xl$?P5 zrhDy`C#&aZ`Us1u_Hc_XgroMAZ^Hv} z3e8!^PtuA`x?vs+m+^Xg(K4<}ZZA`J2S_$%aGU!YYhps*=tHzqb+2#;l{r2XRqfkM zwI86WJ(lo7T%pWSbuSrD!#S6_3xVk)33|~1Wf;7hj>&ns>7K-I{dW$~8GG|U8Y7c& zjx2`NieCD=`f*_}^^q>g?Ilk!eS3-OBm3~AdXDfMoB5~1xU0_%L#U9a(R`_bTGX$+ zG!&CM>p7f>0b9Zw|2V$x&dZVZ+bhi+UxFqk(&)uSG|Rs7b;>Ykma4m)hpLaX!Do1- zx0k8A+o1!!{Nw0#swIvwlX6178^v~e!AhtF^QJ)6-CW5c3P1hFSaq+YM=r&F)>OL6N+B&72ohfcbyyXi0r@(yxkv;&$vc@pkFyekRd&2 zKq&AN<5_MIFUNIK{f9YnH}uja6o=QgycpKACE-3}b|;yXDF-z?^mU9BBaepp>-17km{Sq#C0${&3h% z8)OF@!E@3GkMIqDadF6?o;<*f3KOY>7tso?b=9GaW=J6nS7#j@wsNz35#M!&Ubr=E z^m|OTzp1J{e7D3zZy5w*VG7K}C9=vq$9Z;p4Uyj1MFf7}d;QA!f;%4e&?ZUL-7C<6 zUYv!wI4l(U>G6DD9oEuHUWBDs%p++a_ry$nqvbU{4%hv;_=wvT*1H{i5YIxR+^1LJ zvu}&N&yMq4flM%GeYrX7f8~P2xwiO|Z&H1PPm}5+^|DiMFV#o%_HrT4O!bQL{pe6k zrBZ>bWUZnXOjq4Y8Ag%|!sl|wnBzWWj)^xVxA2*~#~&i#wqUj&^&=QB&=om}(zXwO3~yrjRm>)tvUGU>psJ`=q+IN$Q7rQ@2bX*$bO- z3-6snquBm zZ`DT>y(}?tQExBah+E-+oWv_&xghzasYXcOUbq0Nr50C6;yI3leCiozq~hp{f5Q(o z)tI0c?BGNGY;?^(+RJ^buStniv65Wp|_V*(R{Ys%ko~xKir4-nZFY*;z_jI3--ZDxCD3LIlnj6Ucd4o zFpgBU*ROn)e&vZb1-+n?-d^w)o~Y+&>aL-eB;Hh$z0sb#Im-pXOkUvhjkZ*yOu&)S zAJgitF^rDe3)NZgKrc--qM=tj(Ur&|SOJ!5^fTf@KRnKL-I!2S`xkf*uE1%%y~LO! zUW8lngzWZ`&Z>{#SW|bGD8qmyqWZ{~1ic{btoz%sc~iK_zVcIiiSDXuFF%2(TiiD_ zEGkS*kLIVUr#HiRs(RJ?86p4vxxB+Q8fJ5Lhk2 z&@9b+C8i^qKTNgn14CpiPK9~0#In1UQi@YR zJ;&bC3De9GZ&_ku53A?6pH7D>{$BioK0&JxcGs^Qig1RM;W9}(>pW5OUa6jKsZ+r=zoPwS>Knie@q8D4L5fO-s#>tEd)N#TBX2xR^%cz>woQajfbtSS|>U@-C|9ZJK^l&k>sNGyirz zbf^A294Fu`Uf`BT>oRKE$}?s~6MR9Om&%U3Ym?9BIhyXp+ueq+(;YGN()64Du9?zP z^3_@QrC|}RC7o(iZ%Urn?Im`54N6pdRd;c< z>LZF?qKOGhH7a_+Em3vXQjKB1I~6P!6VO_goWy z#NV1~#B?smm&es^oh+AyG@B==x(lX{$Y<2-ZX^7y?l!2p3vc8LY2K?R4aMExX=l9)_i}^dF{vrh9KR%9m0HtQnK7?zN@}bzjLyn1 zK)>?aazQiMun9Vxb+BBJAZ2l18peg1YJ_U_rZkf+lax7P4p=S--iYcW4ft28G09|` zK4Q)~6yO9l)n3sH%gvigXR_rteAZMWUX#pUxzi};`&IQRREQV64 zpq1fIF37woWsaa}Rm}yR)Lc-!$?pxv>5}OqAD{!hU>r<^d054(l{q@|rs6@qcj)Ap zzrj0v!Z+Q8aMbMy^=@0(Pp9Py-jf&j$-JpVA1RXQxIn704pgVF54G0A^3DUVBWsW?;(xZi&v+Kyc9v>{&9KGZ=*w_No>6o7>5#i(E(}%G z-qhV%Q+G>nGK`jdQgt_Q3% zf2ucyckv2$WU|ehvP?GpP;W{z*{XZ_xpAdm6ILgU5*jOmKp93__tHiQ90$#hY!%Iq zT#>u_JbbE9Q7+tGTD|^-Jp#UK8H8)NqFPFallU4D-3j_Rkhbf z$v;z#kR?DrExUW$vb&aQOl~jDdntMmsy+fkWh`2w#1a$AFeb}r8pJu8YD{uLa8s0F z>@tRN(6YPtd{Z#=!tR!s81H9>1++}+lsRgn0-l;B;_#fdNJ=4U5s=M(<-;v2y^h%ud_FNFpr=_gSvEuJW2?j9q z(!_)up2r8V59BJC zvkvuOsy*J|NA7L>)rUCK=Y)J3ZQj&kUg2s(l`G?EP)NfeN|}XJ<_MU67goJ3Eyj5r1*7gX$w^jbW^p?MWuvazU2KPEw69>F-8~%)lZUhK62f zsYbp>SLGBORMnn#vpMUk?t(e%${Z8)f=XFMYgM&3hLLWR@DEk()tgdhy&b*qGT)}> z@&SIL-d=qrWC;l%;WB)o};B2?TtPt+n^qITXy$`X0j9Xvc$w_%>}h*vUwg(lW{my`e2s! z;Sy&(K`-7+o5NmzBEIPFSo7cqBC4}Kl*g*)2vvG}fqGMu&710lNgwVlKEa!+kH{{m zC+)+X);^r|rhe;Jj+Sb~nQV;`SHlZYb*}<9Anu{gy8ptj#8QoPShmZ*(Q-j}ogb3w zBe5vM$d>}kWG_O~z2Y)IBNWnb$wlkKNi)7w)gG)5r_MSYm-Fq;I!Wl{%vonu?H7}x zR~5`_Pc?$6yLx+pHA-kNZzNTB|7w&3q%7$R!>~}MK^ZP(WsVib97mz{;W9~`^*7;> zzY#aMqq0-hyY2BocUG>7CMG^%8>QmzzCVwE2{bE|yX9e>Umlk`^QNFbba+#o*1f_u zUmtejQ9K{6`^WJc*CMfGx|}FKH9DT;OXCW^GG6PJig}Ja6mv0))6ROh<1WW1-G1Io zn`Dn>vMm?%fxi=Q7x!1Hk=BG&u9D}_6t+eQb++cgTWcPeH^r7}bf?4RB$JJ;Ow?U+ zA)RUr)u!$yta5d0GTC~2A;=F@)n3)z6R^)zdrLLy?FG$bsYdC8 zL&fwFw^Uo7Tw(f1`u3s#-}sGt%s1dX9gTPSdQQBlYwAtW=g?}*F%Qz2Y_~k5GudH+ zE6~=bcxb~fnwY>wK0}Apo5Edk7|!7hWscMmB9|F@QofADVwsHRg?`vg z&|06*O&>WWmdU2QoNknu=P26d4TF68_DawT=LYqr24NrQB8)#o8zr|e?W`w#xa9WY zW^4tLEbIrvq>yH4>oe4LW)?AK7mDh^f1l_wwpFY9?D| zp?Xv6*z#UZTc3GsiHY_`Ns`G{^!jJ2F|FEw?v!#zHr=BdsTPSsYV%0<9MoOcgY$hQVNrCOc1na}$ZrCVW z_4aaC*)rMOiVnJSKR;Y=uc$0z+3&i{712UBTP8>W4v-$00mdBV0XB+xj;IXdi1nr( zg;(w?r*lE7kH~Dx?i#~@8QS{fT-QC2Yh}B=rTl~bmGtW`q=)p3Y zri3}ZGFp{d6D>*2i>J^y9KyXVyPLE=)pIoT(oFWBMv44H64ghBx!UjIeR3Reo)}(l3rn-DDnxgP@mWBEvU%LyyA^cb+@?aN6coeI$IM-{PPL<;BDO z1no`1a<){Xn;j-lf!OVZ5r1~C;s?GloT0<8BdkyDiVpj8d>vIEv0M=4s6N7z%$r&% zwNQ;^Je|hFFwUiJoNkn8E~wpEhdpo{FS7Qg-nk!anQYa)U@FWN%Vg6MnIlt-Irf1r zDB-(*A3t%oS<|YP$#(nsBwi8A1*y85Y!TS+YME?U2^BJ%)SJrZo}^m@+M9Z0o%O@G zOFhQ~z3eOh0(N^T!_dS8EsU4@b@4J^9%i^i_v-F^-0GEK+=px7v_BZOVLk2+N4&Oq zwKw(6^pPAK2zq;&HwCq-+J~vC?j~(su{JN>kxSaX_O4nMoAjI z{>}yUGIdvbQ|U$t?N)Uc4zjjB@A;SUNBRxU+PodPAiplENiB+I`AMwkm5thmOL9Ti zutD^uUe7z+k#Lr-^F!>&WMe*R>$5}_n(n3Q?hH0(-E~*y7=Lsx33t*yoF5ZS&Y0V_Shom#RXo>C<*_hCb3v%O*9Un@x5Gtkl;CDaJL}07L4sbW zd9OmtdsVsGu)-}4B~&b+>L;30LU>H1Q|j7J1);^RPgQ zXjWXFS{|)SEsx6mtWZP+*5*xFo7ZlyB$I7#v^ucY;W53@EdtKK9O%%!(6$J`QjIbc z4ZXBc5Vfcucam;nwgfsMN)c1ikPdw^umk z_rJL`WnN|>~>PQyc?>qs?11(n1T{m3wYdUA^5ggK&a5$NK^zi|f~mb0YJ`fE^z zk)i4?j^uQzk=98yl&flw$}qawSNPcI z&`dTr&=FQ|YC9g%Hm`dSH~BBt`s_jdWrXH}{$AXG3A&`Bo%N*63-`4T#|Lmb)I&RS zd<<`;g+j>my^LWLyHcr;l~fz5{QP*DFAPJBVQAJ~JL|Veb3xXJOWM42$9hvgQqa~V zqi;0WjT2LM|75b0#SQM{ID8{*^WI7_*|<@*;z7#=-NTpgU%4REto@3x(9I4L-AL|l zo%P6?2M?uD(F=Dfb4+wEd?c?dt*ZLS0BwCj2`!3iQgv~)TfoyzAL*x^b!=r_+_;O{ zS>KP_V520R^;_EdO!{!pTLz)3yK|uu(v1?B3hCR+5)*%=8skkqooeJePJ2_o0#FwG z)hJQssGaqs&8wMg+afUZ;&bk3v@@eV-bRPuOh~61A-%YPBSdwta@D=UGF5lE$QML; zDZ9Peb3vx=Zr9E_95sgV2;M*o2J5Wn!ze0_OT2#NwdqvjI2vYoFVx$MR3EWSc6%;J zJL{6Zz1nj@wnbpx6fDs#0vOAKeDAoE6SmvS5);RHH*J(HZ~#whXPsZDJ_1gibr=rg zRkhczoK`5qkcry*bUlN9&sox z_JTAQ1edJ!X^R`}&boZCTu>JFRnIY=>gR_lS}QAIG0w3qf`PV0z_vwTdqL`~yZT^@ z8*+v2%S%l)2IqUk{YnQUzyknIIQg)t1< z3rch^c!PF($zj-~nQYw)!bZODUd0`a5-5;FAE{w&eWo+noacK)E~Bl@`fyHjK?h}< zY;e2dBmR82LE8GXMoD`g4og+F*S#Q~N7g7oZG9sALUY#TlANIZvRO8{eerRBDcnX? zcYh#g-b-&Un5N!TSmRg6i~SsHeGa6wH-+!u3EWcEo_E7W+NRBeSbtabrYzM6c{DtX zCu@}8KQh@NJCzZ)(N|I3>$);WZGG|%Rd+Sj2(Lw#8BDeR(h4i)K5_^y4>C;M z9f^~mgcrFLp_a^>l0qEDxvpD4-x@WiUPO1jHA<3PQ2W}M-d@_~ReeO!i|4o{VU=6M zmFi9DUXb*rEDx;)*?*%KmP@)(f;|#%ieIAkrq0Mg+RnDPp;>!+ zY&%ei?v?LG$Hl4AsKTv+TGqsb>LUrmK>kVQP01zDU75|enfK5M(N(O!wg}q$aHvZv zy0}rQjS?AXiyJJqMF6Ug97A)~m0{r3aMx)c&eUB^HQI{Z0$s6#6}qJ2CIwaPtqjr35LR6?822-!4%3vd$52FrWFGFws^uX!)a1*ID$m@vm}q2BMY z&ibwJ*uM+kh^3P{>((eSXWez=f^@|$?vjG&yZ^vXLGvTWaW8C^&A4AqprMz1z;von z^%0yd^I$2~@=9Zlwz$DLl-?r1$8baJ_Da>q+uR{Ojo09TwLX8N_NMZ{+PqjU%XBY@ z%iS!~NAe*1zxIN*>*7W`bJSEL2CzPysk4cy(tAO&g${(1?n<~z zFV$I3+Pr#u>5_^rZiE_N5nH1KH8GL&;Z)tVwKLku>y0^{u|Ax3*4r?Z^x-TQw2*2- zonIFJb$k8KTa9=O&WpBr-{3c7Wf;lg2A5(btdccUWou{Fhihk!y11daAZzpL8-2pw z=;!ny`jLTA7vDDwF?IK^To6r`QSIoZi3#nj<9XW)+CZw0oHlj0i9Ur^^3o0TrXDXd zTzOpW*M;Sz*WS+@vqtLUcRalELD8~o0Fk2%#P>g5OX7|}PnQoSi$VsG>q($;6v zJa})dPs;_NdXD=wt;%=Yi_lEJK(`2TWvHT;%!g{KjaT>zF3}~GINxbvBIsU_edP_> zS;q~Q-95`!Q9J8jpbb*egZhUNeqxvj3tdgT&b9~==Gd{-=$hjDdVAp^*beowla7XS z?gl@?x6s0|?-b{FRd**tY0~E9Dw?mYPZ^^6Nc&c!rW)xas5iA4_VRImDZWL|=ry&y&8w+Km_lR1KxemC_~zd6W7`Wl3VUc{*s7g%xcVm-#NAxKklt#lA0WiN)3#1e5VlOH|xV`XFa_aBnQn|e;_aEb4aHe z)tgFEjbWYb1YS4Jo73LZ&Gt+-Z57*U z)LalZyH-ZMy%J|VE_2nKW{zN6jqT_qujx@}^ylKkZl^X6L>Eb~(L+w77iM#RR}fF| zvqQ(y?4LfIX0qiY7;{v8gtpn@#!)&SY^#w}_v*wsFvyLLic@pq3erZ2wm#F|6m|v6 zWQS*TM=mOJ#J{#0wNaAXUeZT%K{So$X>SV6S^u*a1eVFRy`b}OOm<JaD}bdmEjB+ zFTO-pzct>H?V$5Y(cc(CtS znpO1T3wQ$d%0}x=or24v%<%*L3h28kZ z-e$MgHQgfMPo~;;Pi`-?1>s=m1+!>E81DM3=g7a^=kU@!2v=Rl((IqLGuIMg*D3C) z=;eyJ1Qw^&-M`R_F#L4Q;The9%W^_*FW8(kO8Az07T&ubEYKDE*pfYAUJ$P4HLfbm zrD<$?L9S~o?kB&;f2^J1KG3zZyYfsIgas%o^mW!K;dz?(!d0+XO7!-^f#yvGYkfY_ z#SJ`aUwKEW(K_qDb#Vjx%W#?yX8Q7IS;|t4Y)i9}NB^~UCh4VFT~g7dS!@wonoYJE zwatsVVpkn3lWkiB9jQjTtBV`jC{fiu@uu8Ueg{8D;?Ai)@gO&rZS5?%y&%0bo9tx& zPmK~dC70f70fGEzQC@+f$w?MKpr@8N;wX9O?$AuALck#A9-?=dGHwE+28YSBLgvmUb21yQd)<%iw zo^r?1tfH6RUO_!aUEHu-5D#~@)yUN@eS3i}lJ%*yPpY-AAgzWi~$yW5T-V|DIY8Y#FS3O7VtXJsT8IB71K8;>zOS9bI zj)tAILAJ?3cUHYAvBeGD$yRR)Ca7w!`UotCg}Rd+hWq|;ckjb*)U@h-cP*TzL#$f_ zJ9TN+^pUra-V36E+~KUNYVXRzG*=jgw(n$XS~cmzX=lB?52uY1?ZerMU2ht~9gUJC z7u3GhD4n!7h3~=>e=D{v039KJttc=9ta}*%rZV>%-YjHs)%oQ8fLQW{z=*n=GRw z-}Qs*bC{kGtCA4X_UZJ zne+GJ29DJYPTk3tZ|Y5a7=G6 zdZOyC1X6}EDCk~LX_5;H$}rNm7i4Lh7n;$$DM&l(ejguC`f#=v1iIBoL#?xJw->FF zME4qF%&`+Ox6nI&On;>s!vR%y{r%`g>QmI3@^Lqp7lykco8YKyONjewQz89pK?2vLjc>s(g-3zJ+HDpV(x&wvjR3o%fdMDeR=KZjhHiq5q zcsTFOS?6ycwgzuojW{-iki;sBAWb;9HHolsA5H_Y=B$INfPEFw2?YD{*r(YHqFrCBKA>25(>ovMp#Xpzi@Ns#<6iwx~e>C$Y}m}-a)`<>z6v;(wJ(r7FH zwsw}R{PRTA#f=(Dul$29so3q6G)m;2>Rw4E8~;WxZ60)nUN|U>c2l^NY$uzS_<7OP z%yGX5r_$(Uo})4h%Vf)D(?@J4TQk|ZG^?#ow8f3~y`WH{`UqJcF2qj%m&N07!=I0j z*j~_f-3!u22{naJF6~XZ5uu38n-Xn(mg8)gC?l}H{9`AZ&dNd2&bqF_cQi`Wn_}B) z#9}BB?ZavFpiCD@WhiKGicBAQE_ZYSKL4OErdD$;yASlb!sJ5=^HWlSYYpj--1*b!KF$*>*`S?8 zb(Ek{f>fhD9W~WRn`Ad1r3-S?JkdT}LENB00;b3n3V@|1@m zlP%4-E;!lr5>xwd!WMHc0>$oPxaFSt_e!yALYUeQ!ppIScE*G8E2k1C8$9KLxDo$j zx5fQ@iY~{y?q&X|B2?GOnumfkioYV^m&IG{te-eh(<`1ahw;KleYn&psmTRlH)Td) zYOk%&Ihx(=8ZFTL%s0A7%G)@5pHl=wETQP5N)h#T&FDACS#DD5oeg1(XH z)E+L2kvuuhwJX%bGa2)d&>1EkE8(koO%LK#d&VEkc2uvc*qHBhM>K2yNZ#6?fo-Fh zc2nBsjjKT1uoL`H*T;9jbQgE8np7iBFSAAV;O>P~BbQ0D(i4e^f5?YkHlmL{uVX?dv+hr1V2jq(|(Mz9*e;EZT}7LAg`YDDuOw#8=M zjsB4dHt%Bj(-fI(n0Ua+1}_MyMtjj8b9=)ky9RAu5J~-ArNIZPi|H<@Vad+%OKq7n z8`I0NAkESZO7oF|7X;dwJTR~2D-qQfWi_JBn@F=LRwHdTd%{tB5v<0joNPEsaC*ft*g5i_q<62xYQ)_O%>(l++_snEG2W+? zf27||rk5k;UIY$Q2jxJ)#Iw{YX%=^{(9N|~!_p5mEY->dY4e~=+(?a**xB@tBS1S# zoNSt`oAoZbS&#Nx#Lf}l=tDZ)-H7G^GTHnu+F8l46qkYnrJeN-+#EYc`NY~NF=+E@ zCcDJR);4dovIyEIkJ!ISv)qoFh&8{$dl;6$3*tSrS(KB_U=iFkFT-aW)DH)j#qd~W zHOdq<@n|1Th`*!tX|z!icbE;K%I(Y#Wtt1p#6+2s%?0fYtxvET(dG>;TsZ>M@vu23ATxk=WqiOkv8keu$1mzIKAQw-8q)~aP&c*A~7MyF!xHFY&uDo z=^i{{!Mi%Xne8OKBl>W&+_JEe_2rm2*+%wX;x|QU133_ zDah`YX=lpGhNC1Rt%}{0I!d@mHNqnnADbf87}~fVYT{ASEG?zkRNhT#`t1`Hr&pr< z$IV7L*>ann^T$|iD?$^~L3+^;)>I=bi0Tnj3nECfO8I{Z3!*vdce_nttr9m*%VnHi zi8PCsqa>Gw-n~+)QTp3%;6UZ%z{$pZq`X--MWY0qZ2BI_Drpvt63o58YD|=WR!+95 z8w*ahxe`v=gAsQxyfBXG3*%{g58?(KCEPO%a%25;yI5AsVms51vqPk(w71|u<;sD& zt#hx$B2Yh^T;jX&x&5SlxTdKO7suP#eyM{;Y?>sBo%Z4Ary7=S*z@6UgWi;;8pCOG z6{K1IN@(-8Bqhz38YP;T057O$^Mbem%0C<>Aa3wR%tr+Ik%BZEm8Ts4Fv@DARZ85L zAVa~)h6R!ED~BKMA3DiOnvI+7-f+xZBHgUZF9QEub3q`@x?;0ldpAWo_d+HcX;p02 zk;w+-Urjt;#1ooAcwr2u%Xl^98FBZC<)AXIW1#sWO^s^w-?O@S49f zTA!^0JYt$^EM>AW-IY$%QdtCF1)OY6HG+1Av?_tPaSJ9M{smr;wB;UA`*7K!e6@PS z3es$BYAZsu`9#m-ZO!iPi(BYFW^2@35Z#5Nggkarv4iUk4is38vP!4BV+g$|rTm8< z`i;JcZ?x3PWYbl1KfE+wMOlr~S-=aLZGXWYk435xlz($M-Zd}cC!?9{*wOrJvyN0F{BSjfC9nw6 z%Td;0=ZMaF%4A1vl(?>WziebaIhz-jlm5!hh}e(2#1^)Gi1ushtp9#`sYmQFEQsjC zaogBkr@NX9ic8FF881U@AK$?>3l;2H9j^2bEQmFvSyMi}kjXBcUZDK@)oxJ$4Zb+N zNLh{HiQc``;acJa;q)@E{O2s@b&(4){lE)KtVVFMk(j6<&89{PHtWCB&OqG2+zXU{ z{kx7-2B)$b)g#tNI$>(>ibjbhCJxeHU^Uu3pNsK{h1FX)vv59-Ilf~bv><+Ot51@xwLH)Zg5{RTf=xa!XMgKkIAMu~wR z?nY4BnW)1RydZp|d7-RCAC4zRyc|1%q=Md5Vi72Vv*2VSF~PTW?p09!!GU6Q)|W(l zqo*eeV#)=97i69W%>~gOvpKABf93ntZh22$lo}(XzU37Q=6uZ0x2IX%_Pld56;rFGnusUL{9~ zj59;whl|)v<^OiL+N}Yt&r5U%v@@4`OzqnjQ~OzP^U6XqJt(UYrx(Mp^v*nqnrZ~A z5r(C3z@8ShAkxY0pqxJa(txtC~DD4a`3%nfpJl_nD-MjFEtxk7~RAb^~|C=<6(SWE(;lm&AKUaLGr}j^5@+#zuRt-H8q)RC6Y$)lr?KV z%ue!iTxBSy8gch>4Fd6Z^Tz9D-TfVPvkn%4x-7JLfK0Zs2>PSV8`UEQ(k%Y2rZXsZ zA?VcpK6Z}qh|yY7%0E0}MrmhaSuKc4gD)GXVOWx$O5DgoHGkx1@QBGVI9!oxMBa=3 zNp~-wyE*}_&pz;oB~~M;M~nxmiKn5liIZJw^CrqaNGharFF0I5`L|s{KTLNMt5N3a zH(FVZ$YkU6qK5_^v46FB)5{TlIBk6njHB$dpp<_zJ5GRM$#wG0TpjNnINA4b_Yx(| zV(ul!>?QDm;s;X=yJhL+XlBO6x>?uGI;lr2T707kWCpZ(_j)C6s2@&Av+^19ku>*8 zFGtcw2}rZ_hty&<%0qP4aeBE8p^A4HkW{XP2j&gwW*vKa*=X=9zd$$Tv3VcSJoue<_9v#hVVu>uSL(wlapN{xpXkGp=7J&|C3gsZxN6YO zvJTa~vZ3H9!y}dqOEd^9g2c&Ybk=Xe;VRlFiGOKtN?DEcH3nDDw?ON&w!>A5RAaIr z!ckJ^&193Z8f!M|v6KEA&V|O* zJ{hRhBlbV&O@*;;Na#uJZPVOk)&7HdMz`gn`r#7o?0`C4@5Se^TPDhX#B_H8E5*(% zq-kMPHZbqz+JuIzn{`+awe`6xZm?B;yF27h>)-V?NGhQo{BW*+P)7;Zyd3*0#SS$1 zFR&mQrJW@UV%()K4BbtcpOFm=)IF4THeEnFTMF8l48aRSni4!>nhV0+D;Aq|-lrZh zoD=8&ALn}Ib1Qdr+(ZyHn(+sa9{ij zhg^`JUWwIcj|J_+p*KZNUl=`=20z^_0!by#r17Hszn@(G)K3 zUL^x{vLGTC#8qe>M0L2POt$`ATXT0Qvl@*yN(gPn2jlKki=i zKK`&aHsZEQ`5yzjC0rIXOU9S(UP!B2uo~}3BB`jOL_6#MU^fL;BVLZ$Ss!A@hnZ%n z&PVhcO|>kDsm+VOE2g{QFMBY81Ep(yVkr+Xy@)uVr zNkw;4*sPaMFa4E|G1Ce^9B5}lH5b&BK|A}D8YMd2-KxY5%>|Ke)?uJdyr3}Hj@K8) zYFaCN87x8-0(1w=Wu0M;THI^ff_V; zaG>ZBT_+{YvQqxzVOCDIz~M^VfI+bXFQ{mg;2SN&QZA^pn@YSOD%!kuPb@guXp}hT z8<}>}gW+(UQ8G|7I8eP~dunRY`uvFbh`SWZyrA@QEHz4~4m@J$!=*+^GEkRtL8cLS zK|wd`iIWY7D`;o%h=I5fzrf9F&{^*W$+-4TCQUyg-=I-OodGudiE)Hbgalz$Ftw^Y)s!E|>GzR~fR0Z;iEEQpkGG2Jau z{;_kEvC1OAZptcgLrJsosNEYh)kp_mpoU=y9x+qb7r7u9s6m5|^X;_6B0#F~|LVgD zctL4C@&+azUq5S=cgY5L{K^Z5D^Hf;+>emmRmwl|UdjtP$lI}V^zeuk25QjGY-8WX zq0Ku+{cw7E>2$X@b(E&IBF`&6<| zG4$bV`Q?betD1OPnBNT4aP#Unx+}F-yCr_*n#q+Q~Xn9iB&5wx=(p0b9% zwR*%x%M^pTS7n%OC&poF7H^@6i7%Mi$IJGV-=F=JuW_3_TA#4xJ&gq~2y-vBTWVq= zV!Asv8{&I%d(+hG-HV@@+u3B zNNHz@G#htWHBjq(L@EDLoL*d55HYn^8hl4-CKc9)kM>2>yO-?at@0mM(kxg6Xp~4b zrn{oMsYE+NCL23PE@)>)NwcDvY_xerDgX9hIBBoQJ$V5JC;!ID);`>VFvX6N0dTm2 zG>hF-d}y!7v-G#w>DRl0ldX2kTAXYmzP=JVXjxU6(~G3=K0D2C73tNfn35>M&e z3)$U0n%ylNC1P!3-`4h&fwACZ!$7Ss$6lJ4$ZVc}$e#P#_L3wP0ch|=s!==Zp45Vv z+PtMqHcUJ;5UEDoy-K;DWS~YSTmE8YH41hZFf0`YY8qiChq>Sdu~Po2KVh?8Ujz(J z_YiH~h}LJ7tT(&D-|k${Og4ByteotiyD3(WSekpKHZQ58glgTr{?&(5%718V+lQV; z9j?4M{%IF`eWrQ2!@s*1Xi|ZXJW^ZkaR-3u1D(g4I~EAlgUqI-diJ z0DU;n&ZuBDVzbT-q-819Xy%9I296Rm5BhQ!0_9(6XG()Vh0c0u?nM_2<|FU(A25q& zZGCq;NJfM5PemqM?_M1P+Pt~@B*@?SDgqq80^*jw=le;~|9nn+vBy`&IoxqC{3iz!Pm#&C6Ljae$}E(r%2V#nB^H6U zc?HwmojTnu4AeE0e`>{DVm~`FOfvHXT$y=1IgZfNtCbp-601@BaIAAL(o`c1)QR%1 zECS7BM`bmdW$?oxlWjYOX0A@2yU+1CDNmX9lb&AbfCE~MP>C0yMU%frJYf0uo~_6m?;0} z2!^E%zAD@4581P!NHx;GIN4xu`Z?JoKg?h^mA=uRl?Gp=8nrjITYFQ=3yMFD)i$qo z)?rxUKjUIM%a3zI0;ao7OobYju$#IRj$0U(U_so6T#zYvK{2zF9^SKG=&NSH6v?}&bk_?c``g=$w18`wKt`W5`)CVL%J4EyTkeRtjezk z#g5J;+8O;wjS}jv)7@!dzFifUQbF8MKb)o-<8yP5YBfsI-HUF=g7OcCD=X#SXeOH% z!y_hxNQoOIj~Ij1cuaRwYvWdi-7=ZQLENa73o>(*xUoQ{8u;M~UXXmZZ~Y^8Bb?X%?sL@{4Z~bd+mvE(cta{6HkWhF8;2%S=UBMVl_&4 zYG)b;0~3!t#%7(4@^7aX-;Xbm$)HQIhLu^N}7vmPefQJVKE-@W7kUKo5h7P%mX zVW~_zD;cQ4Q;y0D;yHvDMqgG=w)k}S5=?jZW4en*iO#)lVQNpQ4~G}V;5f#@Zuw_e zY-fgX%E|7)pq*7meL31o0aN=e@*kt5S-K3D1%1YR#5C8=x^C8y3reKfY@k=#8Jh@~ z#Ur|Duvy=wn{_?C=o)Be{7t93ExC)n94E_MS^pmRXIZT#)+VQXdYT^)ZCb z`f9UObZSq?1=TVvk9tXkPA`yVOD{(wQ7QjfKVR^IV4wykJ8jlWq**vhLSa~fqeT7&aYG(qK2o}SrCbm% zz|OIh$)-*O&$@@hwT3iHWflP_cJ@PrVX3j2#cTE9WQG|R2EoMRns|q~7v>{1yD5WA z_DT5%Bo(8M5^k;}m4cIvMv3Uu9-DP4^MYzpja0>DhpPb%o}@n7yf7@uWS&nTZp?+l z)%1%XZq$=#zQ&jQK(6p#CSYx1;HqzY=^rJ0ED&?0WGTGb=jS}TRX{r&e zPn=$$o&BC_1ScE0AdqHdHPWgy)(i^Wt)?2IPVLe9grg*EwCl7tbsD{?rjVrmbU1$aSd^J;JEWh}RO&DyZZ z?(s*ni(W~yXq4C{vABD|&FlVzqa-RXNKHINsu8=Xa;k9~X`440mf$Gi;`Gw_h_*g? znvCXw)EzW3@++cRe z-^qdq+L^K%+lFoqf7fYo0k7s|VV0Ya56y~9cFf&p55tmjvT=Ix7TLqc;LdJI80{lVAf2!&2`! z#7^)t-BQ2GEee`yOa^KyHtXSryWo#lWf7!4oY9vfeT~u9^DVM2UK=IJKrQMhQQ}4` z(BR`&`^G;ADqUc3Pazt7w^ME=YS*&0O8wXP@&I znL1npc8+T-IN8VOqTJ%A@}8?Hi{+Oi&y+=sxfeF;YM>_VO(Bylx9B1t*FK!`g0NZV zd-A*%tFbRQ*)&HD)bqI@%~G3EE-3Zkem79pq#BL#f9+zX8o z^C~JQJC;eabkv*=H{28d)__HztVa1G(a!YcI6G>iL_J~^lFZ`Z1)WH_pu}p77ww(! z%z_tGqWr@nR-9f)HO7G!o%P1-kjajEdeKn zg)Lr=n0uiQr+zrRF#d~^9Sc@t$v|zCxM9WDb1h?M>dzzNzg~{gSz6lqKElMq>TorO z;&z?dYc2=|>iE{yVl{%NtPIX&VV0c`hjJg%HZN%=JKep^{;<^+oNT#hZilDtWBx14 zd=pcPcBU?i=_c)_+UjoVm%NWp>@5&CK$_LW!~r#nE2~jo7%i}y3d7*Cu$56yFEcs} zaNT@sun3Z2Nv?sDEjws~vIsEUy)F-wMWB5+7^v;2I9Yd&m3D5JXz{}6EG@tyh+ph0 ze=ob5qqDxvlu5HxW;IfgYDA;NF7m70(lFB$sm5fW#y2{VX5(JFH6RzX&z=aE?A=(t zdnM8=?q0}b!)0N|#DUy{%8e4%(`qdd z)d*e?-ByoSnYaNzoShjKStZSevF2ZWIJzwtuvs^o<9~P?9Z=#%!D@s@45`Lg&IN@9 z*|h3W6@OHB%i8z`*3gge#y!rjXXnD-jGbeZY`2H8S@#dDHcHT&;;!63jBr!Da|8j~BnR1&qo}-(%ds*dV!)|E?hS7F9zR|LTXUljV3WqBz?Myym zH$}?H2E`7BB{>e-89k35Y#uV#)VH_F$>v#6H|yG)8kF~_Xjk1RbKxhy2QR3^YOHNo zl9%R745nUaZaas*Zn&Rl=SFRmC=GrtmIYRLc@*k6<1Rk+x za@$_=$J{=@*_LxbFi?Yb7Ar6xfq~jifZY<5e>2UGb_2t|IN5T}9MSnmRs2g66Ib*5 z+3WmECcd6&NnK(;GXkd$BF%Cey?c#v)BFO9 zU%93lG2P|n5mWn5AgR!0O{;371et8)f~?-X;3z5Og0xZc+h!dk6{_YB;fcEyE~uM# zlLd?5Kx&kvKAfo=nww4pyX8cgO_d;SfOZB)iSm?*e$ZQjsrXzhrn~Z6s*&-+XdMej ziP|lf!)~dabv?aETc04!g4LL&yG1TY4NLOV*x1Ol!^<&_G1FzCthUAJg}YZez3iv_ zS;d|Fk~`t|!ewE%%3cBGAFa=ioHpwnskgu^Ue08j;`9P1n|{)JeiCoMBPP4-rhrCC zVW5sL?bi@oy}X43Pk97Qj&rP%W@U28u*3~waeDcy?yNuLcEn`2)VUX$2cn(z*xGcH z0WwOz(W?kV(ut&PUTRKt)hrI;#y@I7+zN;5f9Ha@pq*hi6?L<2#>Hfy{ud`3rx!Tc zDHmk!`RCc^+-KS-iMzl;K=BT+xS1n{_&ayH~!}Ddj&kN-*8EHfWm{xga||EKtMJY{uLRo%Lo> zS&fNy1`8r-c9-^<6CMWY^4v=)c1oHheL1Q}Y@pKMl{8E5({4(R>dUdn1=X7F>U@MX zlTCBchokv4RY@wv>7^ulR0H*bl*tZC`FEW? z+`P3KCFUUfaHep$iqg*FPv`O?lWoTO=^0v|U)p7eq&3pr$I?&WFNjch%pwujy-uuD);KG}WkguS!{N z=Arc|{nBm<4p+>*)FXD#?2vV|5v)c!j~9k{r@g6$woT~9%4$rRY%WrbS*Pk|6?Jm7 zKA$5OqQQ zxKe%By_C-a+F5hkDT1Uj0VEadra+oCe~>y{iDO})j&PLhWX%O3lO3+2QDVL;FQ}F6 z8uaCeyH`pzs>`C2PIn`H1qUj>QgOQaKn9nEl4j|`@3gbn9ULe-Juaj_c`4ky0n^>~ z)C`Ui`h=-{;$&ktC0j`CmZjYkruMNtEA5PC8Q3kg52ps|HryBtPJT~Mc~_UvRBH z91Ww1G@Ep@j?>Ew)SV;dBR|w-aYMh+JMG2*kC?W3`JudmVTrZPTSJ;nxgZ`9``IqO z1!(X_Nwac=&L}6lN-6(GaC*sON)$V6){B>;!E|@2%%TZ)sK;)qsfCH>Q+&=~5x`}E zi~#Ol$J9~sjL?V6ViW3MdMm4Orb%e1a)>Vi1 z{50*R^jEG9S3VX00p(xNo2p}4pv}ul`L~ri-JQat;{dCv#&mjt)rdZv?9`p(4(uEw ze&w(2w;(lVyz9VHoy)toI>b1A_-HM^?$<+&nIMa{4%owRt_^V;H{J> zb})N9ir27#jj-iy$acft5N;r?YJWz-HyWF|5}Cmi?ipB1V>4SP1oM?%#e4Q@fa~)w zUZ*x%Wvlw9*+*ZEj3>1#^=p(XvxsNVSQ;!nOgosfaF!E1>;I^4YHRZT*@Ew-?2y9P zLTbySrJ~yy9)+xZVb1!I66#!JJPFjvVj`doFj614+VT=tCM!e57syfeXBqliz;w`#pkLPY_IS{oj#umh?KwuO!_5VR!=ZK} z-dm}qZPeZnmG33r`7J$;*X=pOmQN+>2{c$lOEPs+FTm4Nqq|t;0N%ik3F!nTcF!`uHK)1V76xCA74s5WY%X zr4=_2(UjCqOmog(%O3eR_InUhI1iBVR3G;;Q{1VIvQ_Y7S^ z&k@$r)NO{*R@BE0YpIgNvTw>v0J##z_>yy( zS_=rmKaAc)4N_9OWh1Yrol&i&Y8L-SWa@=l{hC;5E5jV4DP-jMq^>3Z)x8N1_!^%L zhujWdWjD!gIbtqE<$IZ5k&M2hK_pcty|Ee}&bS%VxO*s7&% zj93s!O;7c#&!EMymV&K{T{0LG(b(#s#umP)M%zuaPUqBWjOSMYWatV5TV8L2?Cxw? z$}8Ypjw9@V(8ab2^&REk9dZUYK*ds*#RmLc(bD#h+}rTo6uzlMW>9kFvVc4FQ0~i} zVl%GG+OHAJN6s*OQ&qAFP9gLh`B`}Hs)I3&sBP#D?yj327tm_6IQ*W3(S$m77-;av zEp`o(FqmhS=!60`euvnr+^>NVZJJlMszra*=mu6Zo;bZE5rhxxH~JyHj$bW-KZ16k z3`6>jEB|YkEarPuG6Sn`ir3>#P0II5^Gr`p`!#5h(Nta#R=A~prkxPMwNHc`u-{(r zea-Ib>BU{#-=GCA!l&RU{kj)ZbOa_ z+ZdR$I8q%@!((qdRH;}?sAy@^0P0rc_-fhmV6-(GV%fQD-iB|MVm)<2_V>dLn0}RD zOkkIcd8QiUNvD7hMYFpG-R8}S?}fA~e~_R2ubXIGSu+tx^Gpt;?`Xa(C-{JDt#K}w zy?+_nfz*qK#AM5Zy5t`0b~ZguQ`JQ zq^DP!2!I0>KA38yVkKkC7@i&$W`E|3vKfAiK~t!$X%el;<44xJ7b!K|tc}}{_X1-A z9SRQ0mC=l+jH6k0Da_(=3W1~opJT&F{FC2@$L^*txH3{jdiP4+zxa*ujn>nPMxepZ z%ke9Zx{a}&LvvG)bOQ_D6sTCdL#lYw?>Rop0ZhMg#*1Dz1Yc(rm^)5N#Sdgg<&aUS33Vj@s-E=N=Hahps2ShhFcoeu23{Q(*_mOi9UOX^ z_R^R#v;*ynaK{wBsYJ*DVUB(mWH4 ziR7Na3j^dz__#GCRcYsX&GB&;ZW-zprs9W4zZK4d^6AA-&=e99@JG6u0Wwmi81P4y z^6WUC3wr}qNYK`&cAH_#GiytPoHCuza${~6dbmMh49&2M^3@fK^67T043r*7!Wbvj z^1&9y7T(8O;#zGAotDdT&%6qsZA3dzY4F-@=5+V^-JFH|h*HCokK1erJKW#-+3dQ1 zXx{R7I9#Pg?CkpI!?VeLu3a9MnK?8m4s(5Tn8oX4iN7i2NUf|l( zF9MNNnSP|ya9$M_yP29+Exdo&)QK8f_+;E~wuS$=tzn-zDVH?I2j{Y*rr_?HAu?7l z1zaTy%(O7tV$#!A$>K7=@wp& z^b08|>@YO_HboQaFpH0;A_-$FqBmc{3v-XI$Vqb`Zlks7@21latF@66M1(-!SOs-hiqn9dOH)M=MoSDJHY=|jV5l5%&K$GN81#!#A% ze2=fK?lAmew=-0kjihwKWX|HBLIy94Zg^pUyGtu&iJ2YLoMk%3rlw9nxA~>J@8MZT z#&ZqKS#*Lf(H$vXj+!&*sodQeVIkP6NWy?I5zwKqCg+dnz{ZPck;!IO?;q}7;hudZ zpCy<&u_+VAR zS$7zr(pJiBnP`TGJ|GBlT_X8viqlJ!8jf!?_+DVEqUQ+q8@Rh>AU3dJfn9|Zayq?C z2SE}BKJFKC7ds3(AY08EY-2#kDfJx1fu5AOyEIc4o7Kt=sg>iCUq+2B@d6)XI1j4S z-f-Gp4R|?zA-o)$Y5Hv#b{KeJfH46&p>*a}QjhT~NAuu1=3ekkS#>T;%G$rvUwQfT zg6mV3@1&FOV5b+tA$(iZI$Pl2x?{ zIhx~B?k>DUcscS`S;K$P0l}8-F1<8gf;Z?*6?ZR2zou|5Tim@su_OJJKZ!T7fz=k- z`nba$_V~L#uzL5Rda(s^24)0J!Uon?W^?n2$l7yfWpEOHk1sRtClHAG#Bt*-yUO zIp5f|4?WPE!rZG&hE~2;Xk@Htzb4#L{>WalCGC>6zgn2HLIpQB9eraR}GPc0YJ5h#e*1j3lH4?rkclU}r zlY<727e*~Y4pj@AMzM|3lg6mgc9khxOO<@2r&r?2DEqCrd&Lv>a=2?=sFfoFk(7JM zU{2iKRdE5V93?^y8~IsK!!LxRW>4H`s^WHgIGpBd^bq`W5#^6`)=gbvziCUG^&1Vo zmlvl#ZcxJ$70bfja8wDxj|qG)Cf&fQ(RQlNy7lgbmNvFArVi3?_7&z{;O^32w60`t z!0BZ_=G7Ihx=}^D>K@r3KgP`n3%NMGcu?#Cr;u2i`w#AUfa~)F;qC*QKrfw3=qvc%C z2~A%#g{W?^fqg*NC>d?5YWQCK6db6?2K1yNC8f>-{9VT*BLI7YcglWy&e+uH>BVc% z6f!5nC3n|9x1VAT=Yh22UXr@ai|p#KfVC-PdlY2oYI#pjG4~=py=)a;7*ghY#b}Dt zOLGR;)S&@3MMji*jzlF>F*dLQ&${lCYrKDO9^e~I185}rHD-l!?I);}1Br<`=+_|Q z2_H8dvfKSyx5e)<1sPgCa5ZS~IKAAUd~`P5FEp!Zah#5(5cia1j0gMeS*?_mdjH}{ zqbX!Ujt|-0_O`bl;?&rZwlOqXx%O>f<%mD+d;i$o3g^vH*)6zxZFe}m@WQ~BjnEF1 zPSV#D&I6dU!bCU1KP>!RIeOm}%J@?LN*qeH=Yw5Ev~Z3n@sW)_E4Zc#qXjfn%%0OQ7F<(K^!uz^+Q z0dJADR5oW3xNVs3Hd8OrP&Xl;<(9=2G@qxy&0D&AB{BoJGN~O{vX;vAf{>$kFLayD z$T*4T(TXAoGhF)EPDo5h6y?hBRrPVB9k>}UN4a2chbIQ@dxH%urThr!NvqLwBp8-J zCu|xWdpHk_k}J^;#C#;~H~+eON&C|2WtN1MnpRZ?r?kc{xnyjiiws-d=JaxmU`&+A z&~>Q=bw&#upZt&I>P7araS_j5O=TGii^`r zecVg^tZYI)1pIS{ButdgjNMd`P{-+oT{4`@AP945y4#cV?+RNUmy<9CP9Ybn%}04g zcR2A!HtTLm8520YK!%n~ZF#+L_d>?gtn}p+GNS>;i7Nx1vXU$H^peB;+v$Z3tnKHA zXOqgmUh}rkp(rp$Qfum5J?z11iqKt zx3BXr6;fTNqIq@4tgjzxk$zi7D@472roR*~M{s4}JOH^88BesdW0?$%xtG*8x`FlZ zanGX_;M#-1DZRN9HxCs~l<5bf?P)rMyB7$;@tC@K)mn0#9S z2Wq%^9`4u@HGEC7H^@Jjd!gqj;Inq1!j>mXc$Jcm^yP@tix^h5@YsR2QU?KZkP}u20=Ln&g{8-m5TY6@(n6hSwNd(4jDY%h_a% z*Lfy9>z&oJJ{g{MNe!^!)HkMzkT-vNt$Jh(`k!)7JBwOj{+o>`# zB5U8uHW0$;MNe>g(NRsmt(P6zE{+jqbsQ4VSpPQl*~X&XcmEM-ouIVJUKoj7iGGdj;m!JuJ|w4V*sAC@9ESD2kG%L{WBa(wCK zDEPZR#>=s^o06S$gbQma|Ip?^OX_L{fc=){UU8P4=!a%~syk-z{s}zm1}Ul2*iG4W zVUwniFGkG0KKNgm^$ks1>Jk62W1<>cv;mgpUT6wA_@=@$b2nbHCp>&pYj_)%oy*93 z8SU3lZyFL|Y*}pd^rF$`58u_b31F-K*T&vdbKe=lLc-3Gdv`K*={{{=jsXVQi`4dtp9OA%SX{ ziuo^1Ozbr$M6>ov$Z=e>14%vWvRb+JO30yh%C*ns1EGg;k&c_aVKZ#Bq=^Z>Pp{$^ z7ZaI*`_M4FFk~sxsxsM(3jK|4)`{eYc^3-qE_PGcth2rx(G-&JhGIQ##a*;Fg@pQY zQzqn4YH4dbkk!}{QpR&Lr4B`EX;#SrQh+u5eUA)#(x_&eF< z{Ddn`FExu_j(6Z(#%A3%4ee~NpvD$PVxoL{No#I|sXdG>wa5%rXlaw?g5dqbZVI^| z>0=7#0WX)-0PAny6vAfxyLlTP+Ux4$hOuR1+(k#sd4D5&oWIR}=8`qYTV-AI{@JK} zvYQ`Q&@!ES4GX;uc8=IhebL?2Wjc+iy^{PFPakH%TQx(C*84G2m1|-3AJ+Q zbQf9sSe;y-soR|HUIGpj$k4DiMA-5|(QTHQcsaTS`4l%Q3^2&?;aC2nCZSHCVpZ9l z;V^oRV5{Qg2p>18Z;B@CZfY5JQ*LA(Vmm|`u+S1m|y5OsHQeB^G!B%Db%J1^?i0Q7LUfe4VGDyEI#_1KNWn=O` zT#tYhvToKb<|7xWIK6l~A5x0l19=H!ysHzMaVPGJevQm#OFLdSkI@ujY+zf(E^79eqGpf6T1tJeS$B2v=)MLuwty=WVQkq% zd--_0V3aWter2}0Vb(Tu=Yc#XsE?Z$>o>j@{TVK7s;&AnuOxdbnaK^gaf;&ge1@T<2jD>TV`@wD* z>n5gOv70&!A2;q^$yy3JA?73KHp8~7gnwV%E z>Kg)gmmY&Z;*M1Bs@RyXcgpty2g<$;X!9ypM*5*0sLg}QIGZM>yO(Q`*Ks0W<(1L9 zS8^WEX4!3y#S5k&HBPUT@l10sJ1z`1O32Zj<0n%q7gVAX;`HJd^qG8Vvo3{? z8{R+sU1=bk%eZ@mpY}t1!o}T7Hfj6*pgj|=8cj9Eboc5MF|}7}IMS+Sav0(IhtAxJ zK@k2fZ(wYZvvSz%jOz)1*S|rA#@);RuoUZaIn}6B`=vO&+_1cN*3lQaAe>%wMNXN6 zaT~2OTf$y<+!v>p$%C;4H9U@S)ANPdpMH_S)P4~6q;}HSIFKvjGrpr%wSAPPy9WvG znR`ZGjxodBOM0p66V&j+`-i5Gv|{kRe#*NVawT?CWpmb}_}YH=lGV#wSe<*tNp`NU zbeapo-OG1!Ej%``U(Flr99fMm5!2m0cwwL^1hy)}OVmcX(H~}Xm}(aUBqnC^co|H+ zxkGGXD}vhc&^#dUN4D`=y?Y%;hXVG7m^+s@&f0r;iL~2XVyn7=u{*a#s*$SA2mds? z?Jt<)v{yEP?_~;WDZj#Q3fx`lM16QzoSQ? zS%VG*7@YczrlMct=H<(?Wq!7q$i-&e)Iq8dzA4-L)7a3L#Om~wLvPWtM z`UO^ZQ{dW5Plpag=5tdbSDuLbcq^|BTitS_FFmCs4vIvnA!)n zzrs8BG{2c$$d6_)+O7}V(O;FT_Mw06kV3A%SMpq*tR0HZ;Cor}-`tz< zkgxOEc-ZdrRoTY8Oef^`NHxX=M(18;lzP@z@KUNpCo~nhn|fh&v!2?4VYA!ki(C-W zZ`Yf1!<-3>d z8WUsUCw7kd!br{oy?e=7zAg{VTmP+se$}gJ={uYLq`N6v9>G6Xu03}GK{&?LuTdxD zp}5^vfiVH6kebE8m{8||Rp)_nWi%HgNWUR5(L|i_>GU#};aNvQz4UT4g)ts4$JoyG z$_Kk~aR!0%zkm{hvpcf(Y~nBXA%9|TQ8E>8av&e+-HRS1#sp~axO=hoYh)Q~COf%j zNOx2ED=&6avID6`*#&z8-z4-Li%fQM9>A6-D`0P+2_OhlN1g6QDVzs%S`MHCi|MXC zE*IrC_~)Fn4UMd}19A6~TB%0Cow~>cndfvTUNR@*Ub7We4!_@>@|WE`|H6Io!B)hk zreiFhUS?&SkDVj`p?NP%cYhc(_;2!sc*N|E8+kqNltXluug3@W4f0;0u4ze~V_|H; z&Jn4`sC+L5V}j(Xvfr*6%>~6OyUFiX2B*4bX=Xe5-gcO1gI|`H>IfyL5PFV;`3P&j2EHlA z)P9akkfE+`b;pY4zSykC`}S%;cK0t@$D5UF4}$PBB?y~jY%v36G)-rGqt*MzgY{Pq zzE{EBMZbpk^VXuJU7TM0IrRF2Y~QZ5M7-DGb- zVuD+sH$}r)+krawnvA&@UKp;P7yCWFhL?!cOT_C56uYCWjESJRAS!Hm>F#A$1~tap zp`jOEj;zyNdP(=`N^%~=HFitD%kd(t9R7pLgXV&yhYYl1{dBiDDESDdS1FTia`dLs z%MtyW(&?rA^OyEZ2&RrSCEcv^1hfPF3R}2mU@e7H=r=0XZrT{vj&w3DLLDc_1<8H6YR;r~peP~7DIteGgrCm%hOSM}OtzYeSLxJ#yr93@-ePJW zFtx{S3R!#ItZ%jZ(X1YsD3F$X*?FXO%mNMD!O_`gps)b!{ zQ(n*s<(fP&dV1;0vD^-nO8m;*q zx=W*BRpV8qTo7x|kt!&pb|6kK*-v`+0wKr#6Ydz;8*<}7B#k|IFpZPxteI?@PGh9j zZi?|Mza1~Q(+Bie6tLU`60PRI-K zs1ix5z_rH<1DR})p^Hp5ng{Be!rY4{(+Ci9Vhd9*id9=4oChFR(t4AO@#pN#_?Y4S zEzjh7=UnIUYpuyPc+AgLxtjj0`WQ=o=}o>b0c6oeeU!uP{V*c)JMu^nR#H5_-ZDF$>x{L1SaF1bF@`b0ZW)<;b>IweEn z^rHF%A9p`HLW!i6*f}y@7=87HQ78GhVYG#hJ2gs-^1aY=#NEqt)+ldn3+I8r?6IUo z$SLK5!YB8_-*uORX0kEewfp@EcRAcae>LW%b|8FH+HGD%3vu@ff7tFI2;=URy3MTA zaKFh_`C5#L@P&ozhUPAriq-Y0xgfQcmU2P3d!vLg(c=p}+bgiffR! zbVavWJ?qQV%AwxBh!;knul!2x!Ip>9%d87!#)Q$$y3W1IK5kwls}dQS1|#ofi&SHY zD-ql}|6E8bJ+5E(p6RDi{-ZIU?^R?+8RvOm}zjCaJ>R3tW3C z_u-@kc2hKhG?Pus1$Qq{>^i0S2zFEaFkZE1gStL7?^PJ%!#n#cV!GR=u;oovQ}I$p zqeRVF(iEo`e&yy~yo|e-8e2d<0%PJ17!$_BmY19doQy57TTTa^P|47Z8B_a@1XJ;) zaLoLkYJ4Cs;d8{x5t-~#x4A^ffwk1O5=~5i{bp`N@JDubkppkl2vXbK&Jl>?dV_zTSgYAfApxH3pJ z;^nC3tR|(q7npu>qTucp=B#8bjoxIi@b@hcP8jm>Rmug(JlA9tmlr<=QT%rh(F5>{&|ng?iU*QR1=E@&-UpL`-H-^+ZYT$EfXJ<{m~vq##j$AS1ptF^S6 zKBD!R=3a5LT^F{QgK`RYFF}3;Jx7o$`}iSlLYT!%d1cg8qaEP8y4ImSYc9w<(LUVK zxXWw=AqQ+#5OP2##6%!y7i9LxQKi^D zL8Bziy^^tort!kK(k=-z4CW)fbUu=md{Yso;^Kvo%vq()dTBRRHnyxZ3&T{jv`u#^ z`f#jdhGc9Zw0XD7L31WtaS!|}18XU~%^hvuFw{;x?ZffasBK=V0-cbK^QEY# z7h0dOktt5EIL$5!D@}Q`&h4oQX&;WB!#4${pKQ^2CR*B;(b7)2AZe<-DLuV#_e!T1 zynhxPD2CCNHPsl8SWvOnhaKRb;`rBlS(iJN%D1S=u=!?;p%r z$YhH%O+$Ov%MUi=X=Yq(S4J>6N5L#^+r)-!aQ7-1Taaqx^`%TUjPc>Cqr6Vm!gqob zQZw00xiamhkln>*9f=A3Mr)%4^O3@chFs8N^Dh42BGm|TWt;*wzFi6iXBeUsJ54px zmy`?g1^X>-wmSC;w^^Ob#wk6ihtiW~#D&P((+un!(L6|}m(fgixM)v=y>?4nOMe-) zH-vlU72GqS!Z!0AvOX}{2DEuWodo}UKtyLB^hZ+7do zH+3dn(W(7cun0<LuC~)VZwu^CFXtT#y|Q6PaPT zX6+}qVWBrtjZ%Scw7fL;!xc?TY^N&O!h1FEb&Het52oTa1c&PwnPwLGmD%Eax*Hb< zQBQ6UGBk2Q{5;-KQ}O<=#jGo}dE;GE?!zT&IE{x@EuCKQ{*f{`QzqNiI1e`Q26Wbu zm;hr!t)+VRLSiD#N4PSg%{x3fm+^AcZ}daH9?mGaa)WHr`N)NM!#+{|Ifn+WjqmFI z@T1&RKi{qNi|s5KM?*n|PN!G0<>BrHYIt00xB0#9M7X5J$DMprg8azJ`6n+_%OpPCTwHWJ;TLr zD*nVbnhWkOZ8qzJX0nZDvRO0Pwfk_i45t@OPMK_JLHOVJuFV4_ZpaSW$eS?TEoHLN zC{d$rHy$7()%Cd|EVi&W)XHSj7j)L?3WFNHUDnAKyU!o9m*loQ1&g5e>1Aj51#qOZ zo?g_G+ePie(MQHN`jQ-{eY6!uG7N%nKrg*(oUUQc^dZ;D*dVR(t`rhv}+ISYdDJNhAF8U%eg@<^Tne~n+_X8Ypw0-X@K zAVPiwr`JJ?ZgW9~=G*b9)jk|Gj%}>^rnL2m+lKPbO;2g3<}8pa)o4p67&{E?k_&r- zX6=KfRW;Q}c@)z+tO*5wHA!RaMC_;5U9uc1-mzdG`Dvle+L z*Ut}klfoQRNtIFWUanv4ghojsZY19n9R_75Zlc}hXgII)k@r&UrqJe1Mq9HIB$ae} zaRdHP~r`n%bc{G>Y&!IjZWHd2jfl!R(q=I(-?!y;n!84t>pBD-^M5Qj=$p0`J?V&IKda`20fq` z{62o;$XRS)T1tEB!TsY9Ge*Yoh~i&pO-*8jWnb;Sgtz>hAH1=z1KBiaf!X3DEY-$_&3Tq|GFZ(@w0Q8q>lJOxsm*!u#jbY4 z!ZT)@><}OYb8lOH@W#p~9=iLBtAFbkSs5w?=+-cBh+f@KoV?q}7OmW2M!8fVGcxzs VwQ2KIs$3-ONOGe9gW(vXV1KJl2V?*M literal 0 HcmV?d00001 From bae04966f912d83d3ea5c40622a45c0d452c31a3 Mon Sep 17 00:00:00 2001 From: David Rowe Date: Sat, 24 Jun 2017 14:35:28 +1200 Subject: [PATCH 37/42] Add "REC" indicator --- scripts/developer/EZrecord.js | 67 +++++++++++++++++++++++++++++++++++ 1 file changed, 67 insertions(+) diff --git a/scripts/developer/EZrecord.js b/scripts/developer/EZrecord.js index 593f9d5fbb..f8dd7bc26e 100644 --- a/scripts/developer/EZrecord.js +++ b/scripts/developer/EZrecord.js @@ -19,12 +19,76 @@ tablet, button, + RecordingIndicator, Recorder; function log(message) { print(APP_NAME + ": " + message); } + RecordingIndicator = (function () { + // Displays "recording" overlay. + + var hmdOverlay, + HMD_FONT_SIZE = 0.08, + desktopOverlay, + DESKTOP_FONT_SIZE = 24; + + function show() { + // Create both overlays in case user switches desktop/HMD mode. + var screenSize = Controller.getViewportDimensions(), + recordingText = "REC", // Unicode circle \u25cf doesn't render in HMD. + CAMERA_JOINT_INDEX = -7; + + if (HMD.active) { + // 3D overlay attached to avatar. + hmdOverlay = Overlays.addOverlay("text3d", { + text: recordingText, + dimensions: { x: 3 * HMD_FONT_SIZE, y: HMD_FONT_SIZE }, + parentID: MyAvatar.sessionUUID, + parentJointIndex: CAMERA_JOINT_INDEX, + localPosition: { x: 0.95, y: 0.95, z: -2.0 }, + color: { red: 255, green: 0, blue: 0 }, + alpha: 0.9, + lineHeight: HMD_FONT_SIZE, + backgroundAlpha: 0, + ignoreRayIntersection: true, + isFacingAvatar: true, + drawInFront: true, + visible: true + }); + } else { + // 2D overlay on desktop. + desktopOverlay = Overlays.addOverlay("text", { + text: recordingText, + width: 3 * DESKTOP_FONT_SIZE, + height: DESKTOP_FONT_SIZE, + x: screenSize.x - 4 * DESKTOP_FONT_SIZE, + y: DESKTOP_FONT_SIZE, + font: { size: DESKTOP_FONT_SIZE }, + color: { red: 255, green: 8, blue: 8 }, + alpha: 1.0, + backgroundAlpha: 0, + visible: true + }); + } + } + + function hide() { + if (desktopOverlay) { + Overlays.deleteOverlay(desktopOverlay); + } + if (hmdOverlay) { + Overlays.deleteOverlay(hmdOverlay); + } + } + + return { + show: show, + hide: hide + }; + }()); + Recorder = (function () { var IDLE = 0, COUNTING_DOWN = 1, @@ -75,6 +139,7 @@ // TODO + RecordingIndicator.show(); }, START_RECORDING_SOUND_DURATION); recordingState = RECORDING; } else { @@ -93,6 +158,7 @@ // TODO + RecordingIndicator.hide(); } recordingState = IDLE; } @@ -103,6 +169,7 @@ // TODO + RecordingIndicator.hide(); recordingState = IDLE; } From 3de1c0a312ffbd10d0fb78d3b418a1bed1d184cf Mon Sep 17 00:00:00 2001 From: David Rowe Date: Sat, 24 Jun 2017 15:57:57 +1200 Subject: [PATCH 38/42] Make and save recording --- .../src/RecordingScriptingInterface.cpp | 23 +++++++++++++------ .../src/RecordingScriptingInterface.h | 1 + scripts/developer/EZrecord.js | 18 +++++++-------- 3 files changed, 25 insertions(+), 17 deletions(-) diff --git a/libraries/script-engine/src/RecordingScriptingInterface.cpp b/libraries/script-engine/src/RecordingScriptingInterface.cpp index 98838441d2..7583f562e6 100644 --- a/libraries/script-engine/src/RecordingScriptingInterface.cpp +++ b/libraries/script-engine/src/RecordingScriptingInterface.cpp @@ -8,9 +8,17 @@ #include "RecordingScriptingInterface.h" +#include #include +#include +#include +#include +#include +#include +#include #include +#include #include #include #include @@ -18,13 +26,6 @@ #include #include - -#include -#include -#include -#include -#include - #include "ScriptEngineLogging.h" using namespace recording; @@ -188,6 +189,14 @@ void RecordingScriptingInterface::stopRecording() { _lastClip->seek(0); } +QString RecordingScriptingInterface::getDefaultRecordingSaveDirectory() { + QString directory = PathUtils::getAppLocalDataPath() + "Avatar Recordings/"; + if (!QDir(directory).exists()) { + QDir().mkdir(directory); + } + return directory; +} + void RecordingScriptingInterface::saveRecording(const QString& filename) { if (QThread::currentThread() != thread()) { QMetaObject::invokeMethod(this, "saveRecording", Qt::BlockingQueuedConnection, diff --git a/libraries/script-engine/src/RecordingScriptingInterface.h b/libraries/script-engine/src/RecordingScriptingInterface.h index a9fdf1deb4..c4220958a2 100644 --- a/libraries/script-engine/src/RecordingScriptingInterface.h +++ b/libraries/script-engine/src/RecordingScriptingInterface.h @@ -65,6 +65,7 @@ public slots: float recorderElapsed() const; + QString getDefaultRecordingSaveDirectory(); void saveRecording(const QString& filename); bool saveRecordingToAsset(QScriptValue getClipAtpUrl); void loadLastRecording(); diff --git a/scripts/developer/EZrecord.js b/scripts/developer/EZrecord.js index f8dd7bc26e..382633f4f2 100644 --- a/scripts/developer/EZrecord.js +++ b/scripts/developer/EZrecord.js @@ -136,9 +136,7 @@ log("Start recording"); Script.setTimeout(function () { // Delay start so that start beep is not included in recorded sound. - - // TODO - + Recording.startRecording(); RecordingIndicator.show(); }, START_RECORDING_SOUND_DURATION); recordingState = RECORDING; @@ -155,21 +153,21 @@ if (recordingState === COUNTING_DOWN) { Script.clearInterval(countdownTimer); } else { - - // TODO - + Recording.stopRecording(); RecordingIndicator.hide(); } recordingState = IDLE; } function finishRecording() { - log("Finish recording"); playSound(finishRecordingSound); - - // TODO - + Recording.stopRecording(); RecordingIndicator.hide(); + var filename = (new Date()).toISOString(); // yyyy-mm-ddThh:mm:ss.sssZ + filename = filename.replace(/[\-:]|\.\d*Z$/g, "").replace("T", "-") + ".hfr"; // yyyymmmdd-hhmmss.hfr + filename = Recording.getDefaultRecordingSaveDirectory() + filename; + log("Finish recording: " + filename); + Recording.saveRecording(filename); recordingState = IDLE; } From 48caad1ad252d7841fe1fbabc87f3d9a688412cb Mon Sep 17 00:00:00 2001 From: David Rowe Date: Sat, 24 Jun 2017 16:03:55 +1200 Subject: [PATCH 39/42] Add user activity logging --- scripts/developer/EZrecord.js | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/scripts/developer/EZrecord.js b/scripts/developer/EZrecord.js index 382633f4f2..7fdebada79 100644 --- a/scripts/developer/EZrecord.js +++ b/scripts/developer/EZrecord.js @@ -26,6 +26,12 @@ print(APP_NAME + ": " + message); } + function logDetails() { + return { + current_domain: location.placename + }; + } + RecordingIndicator = (function () { // Displays "recording" overlay. @@ -169,6 +175,7 @@ log("Finish recording: " + filename); Recording.saveRecording(filename); recordingState = IDLE; + UserActivityLogger.logAction("ezrecord_finish_recording", logDetails()); } function stopRecording() { @@ -239,6 +246,8 @@ } Controller.keyPressEvent.connect(onKeyPressEvent); + + UserActivityLogger.logAction("ezrecord_run_script", logDetails()); } function tearDown() { From 05f0ec267e2f1fadbcfafbd4f1166928b73acfd3 Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Sat, 24 Jun 2017 18:53:00 -0700 Subject: [PATCH 40/42] fix an initialization order fiasco --- libraries/controllers/src/controllers/Input.cpp | 7 ++++++- libraries/controllers/src/controllers/Input.h | 2 ++ libraries/controllers/src/controllers/UserInputMapper.cpp | 4 ++-- 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/libraries/controllers/src/controllers/Input.cpp b/libraries/controllers/src/controllers/Input.cpp index 6f8bd547a2..dbc9071f43 100644 --- a/libraries/controllers/src/controllers/Input.cpp +++ b/libraries/controllers/src/controllers/Input.cpp @@ -9,9 +9,14 @@ #include "Input.h" namespace controller { - const Input Input::INVALID_INPUT = Input(0x7fffffff); + const Input Input::INVALID_INPUT = invalidInput(); const uint16_t Input::INVALID_DEVICE = Input::INVALID_INPUT.device; const uint16_t Input::INVALID_CHANNEL = Input::INVALID_INPUT.channel; const uint16_t Input::INVALID_TYPE = Input::INVALID_INPUT.type; + + const Input& Input::invalidInput() { + static const Input INVALID_INPUT = Input(0x7fffffff); + return INVALID_INPUT; + } } diff --git a/libraries/controllers/src/controllers/Input.h b/libraries/controllers/src/controllers/Input.h index b74ad48c6f..3ca4076de2 100644 --- a/libraries/controllers/src/controllers/Input.h +++ b/libraries/controllers/src/controllers/Input.h @@ -83,6 +83,8 @@ struct Input { using NamedPair = QPair; using NamedVector = QVector; + + static const Input& invalidInput(); }; } diff --git a/libraries/controllers/src/controllers/UserInputMapper.cpp b/libraries/controllers/src/controllers/UserInputMapper.cpp index 79f4325ae6..29f011fba2 100755 --- a/libraries/controllers/src/controllers/UserInputMapper.cpp +++ b/libraries/controllers/src/controllers/UserInputMapper.cpp @@ -47,8 +47,8 @@ namespace controller { const uint16_t UserInputMapper::STANDARD_DEVICE = 0; - const uint16_t UserInputMapper::ACTIONS_DEVICE = Input::INVALID_DEVICE - 0x00FF; - const uint16_t UserInputMapper::STATE_DEVICE = Input::INVALID_DEVICE - 0x0100; + const uint16_t UserInputMapper::ACTIONS_DEVICE = Input::invalidInput().device - 0x00FF; + const uint16_t UserInputMapper::STATE_DEVICE = Input::invalidInput().device - 0x0100; } // Default contruct allocate the poutput size with the current hardcoded action channels From 960017ddd704cfa77321a1edcc6df25e91330595 Mon Sep 17 00:00:00 2001 From: Bradley Austin Davis Date: Sat, 24 Jun 2017 22:22:47 -0700 Subject: [PATCH 41/42] Fix static plugin initialization on Linux --- interface/src/Application.cpp | 10 +++++----- libraries/plugins/src/plugins/PluginManager.cpp | 4 ---- libraries/plugins/src/plugins/PluginManager.h | 17 ++++++++--------- 3 files changed, 13 insertions(+), 18 deletions(-) diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index c80626ad5c..0951e0973b 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -486,11 +486,11 @@ bool setupEssentials(int& argc, char** argv, bool runningMarkerExisted) { Setting::init(); // Tell the plugin manager about our statically linked plugins - PluginManager::setInputPluginProvider([] { return getInputPlugins(); }); - PluginManager::setDisplayPluginProvider([] { return getDisplayPlugins(); }); - PluginManager::setInputPluginSettingsPersister([](const InputPluginList& plugins) { saveInputPluginSettings(plugins); }); - - if (auto steamClient = PluginManager::getInstance()->getSteamClientPlugin()) { + auto pluginManager = PluginManager::getInstance(); + pluginManager->setInputPluginProvider([] { return getInputPlugins(); }); + pluginManager->setDisplayPluginProvider([] { return getDisplayPlugins(); }); + pluginManager->setInputPluginSettingsPersister([](const InputPluginList& plugins) { saveInputPluginSettings(plugins); }); + if (auto steamClient = pluginManager->getSteamClientPlugin()) { steamClient->init(); } diff --git a/libraries/plugins/src/plugins/PluginManager.cpp b/libraries/plugins/src/plugins/PluginManager.cpp index e90d3e3a0f..18ac905ef1 100644 --- a/libraries/plugins/src/plugins/PluginManager.cpp +++ b/libraries/plugins/src/plugins/PluginManager.cpp @@ -23,10 +23,6 @@ #include "InputPlugin.h" #include "PluginLogging.h" -DisplayPluginProvider PluginManager::_displayPluginProvider = []()->DisplayPluginList { return {}; }; -InputPluginProvider PluginManager::_inputPluginProvider = []()->InputPluginList { return {}; }; -CodecPluginProvider PluginManager::_codecPluginProvider = []()->CodecPluginList { return {}; }; -InputPluginSettingsPersister PluginManager::_inputSettingsPersister = [](const InputPluginList& list) {}; void PluginManager::setDisplayPluginProvider(const DisplayPluginProvider& provider) { _displayPluginProvider = provider; diff --git a/libraries/plugins/src/plugins/PluginManager.h b/libraries/plugins/src/plugins/PluginManager.h index cb011392a4..08fe4fde20 100644 --- a/libraries/plugins/src/plugins/PluginManager.h +++ b/libraries/plugins/src/plugins/PluginManager.h @@ -33,16 +33,15 @@ public: void shutdown(); // Application that have statically linked plugins can expose them to the plugin manager with these function - static void setDisplayPluginProvider(const DisplayPluginProvider& provider); - static void setInputPluginProvider(const InputPluginProvider& provider); - static void setCodecPluginProvider(const CodecPluginProvider& provider); - static void setInputPluginSettingsPersister(const InputPluginSettingsPersister& persister); + void setDisplayPluginProvider(const DisplayPluginProvider& provider); + void setInputPluginProvider(const InputPluginProvider& provider); + void setCodecPluginProvider(const CodecPluginProvider& provider); + void setInputPluginSettingsPersister(const InputPluginSettingsPersister& persister); private: - static DisplayPluginProvider _displayPluginProvider; - static InputPluginProvider _inputPluginProvider; - static CodecPluginProvider _codecPluginProvider; - static InputPluginSettingsPersister _inputSettingsPersister; - + DisplayPluginProvider _displayPluginProvider { []()->DisplayPluginList { return {}; } }; + InputPluginProvider _inputPluginProvider { []()->InputPluginList { return {}; } }; + CodecPluginProvider _codecPluginProvider { []()->CodecPluginList { return {}; } }; + InputPluginSettingsPersister _inputSettingsPersister { [](const InputPluginList& list) {} }; PluginContainer* _container { nullptr }; }; From 08784ff30c6006d4225d8b0d2a1718250cdcd049 Mon Sep 17 00:00:00 2001 From: Brad Davis Date: Sun, 25 Jun 2017 02:34:58 -0700 Subject: [PATCH 42/42] Fix toolbar button mouse click when they were added via the tablet scripting interface --- libraries/ui/src/ui/TabletScriptingInterface.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/ui/src/ui/TabletScriptingInterface.cpp b/libraries/ui/src/ui/TabletScriptingInterface.cpp index 8b3dc342e2..1e426dd8f0 100644 --- a/libraries/ui/src/ui/TabletScriptingInterface.cpp +++ b/libraries/ui/src/ui/TabletScriptingInterface.cpp @@ -623,7 +623,8 @@ TabletButtonProxy* TabletProxy::addButton(const QVariant& properties) { auto toolbarProxy = DependencyManager::get()->getSystemToolbarProxy(); if (toolbarProxy) { // copy properties from tablet button proxy to toolbar button proxy. - toolbarProxy->addButton(tabletButtonProxy->getProperties()); + auto toolbarButtonProxy = toolbarProxy->addButton(tabletButtonProxy->getProperties()); + tabletButtonProxy->setToolbarButtonProxy(toolbarButtonProxy); } } return tabletButtonProxy.data();