From a10b157affddd7bba5a294ffa63ef310a953b6a2 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 4 Apr 2017 17:26:00 -0700 Subject: [PATCH 01/17] First pass at having an explicit Hips IK target. Also, AnimManipulator nodes support setting position and rotation on a single joint. --- .../resources/avatar/avatar-animation.json | 23 ++- interface/src/avatar/MyAvatar.h | 3 + interface/src/avatar/SkeletonModel.cpp | 4 + .../animation/src/AnimInverseKinematics.cpp | 143 ++++++------------ libraries/animation/src/AnimManipulator.cpp | 95 ++++++++---- libraries/animation/src/AnimManipulator.h | 17 ++- libraries/animation/src/AnimNodeLoader.cpp | 33 ++-- libraries/animation/src/AnimOverlay.cpp | 8 + libraries/animation/src/AnimOverlay.h | 2 + libraries/animation/src/AnimVariant.h | 9 ++ libraries/animation/src/IKTarget.h | 3 +- libraries/animation/src/Rig.cpp | 11 +- libraries/animation/src/Rig.h | 2 + 13 files changed, 205 insertions(+), 148 deletions(-) diff --git a/interface/resources/avatar/avatar-animation.json b/interface/resources/avatar/avatar-animation.json index 975f01855d..daa69b95f7 100644 --- a/interface/resources/avatar/avatar-animation.json +++ b/interface/resources/avatar/avatar-animation.json @@ -50,6 +50,12 @@ "type": "inverseKinematics", "data": { "targets": [ + { + "jointName": "Hips", + "positionVar": "hipsPosition", + "rotationVar": "hipsRotation", + "typeVar": "hipsType" + }, { "jointName": "RightHand", "positionVar": "rightHandPosition", @@ -91,20 +97,27 @@ "children": [] }, { - "id": "manipulatorOverlay", + "id": "hipsManipulatorOverlay", "type": "overlay", "data": { - "alpha": 1.0, - "boneSet": "spineOnly" + "alpha": 0.0, + "boneSet": "hipsOnly" }, "children": [ { - "id": "spineLean", + "id": "hipsManipulator", "type": "manipulator", "data": { "alpha": 0.0, + "alphaVar": "hipsManipulatorAlpha", "joints": [ - { "type": "absoluteRotation", "jointName": "Spine", "var": "lean" } + { + "jointName": "Hips", + "rotationType": "absolute", + "translationType": "absolute", + "rotationVar": "hipsManipulatorRotation", + "translationVar": "hipsManipulatorPosition" + } ] }, "children": [] diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 097d3a1059..b5bea23aa3 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -553,9 +553,12 @@ private: void setVisibleInSceneIfReady(Model* model, render::ScenePointer scene, bool visiblity); + // AJT: FIX ME... reorder this +public: // derive avatar body position and orientation from the current HMD Sensor location. // results are in HMD frame glm::mat4 deriveBodyFromHMDSensor() const; +private: virtual void updatePalms() override {} void lateUpdatePalms(); diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index 0c11fa456d..e26c339fb8 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -119,7 +119,11 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { headParams.rigHeadPosition = extractTranslation(rigHMDMat); headParams.rigHeadOrientation = extractRotation(rigHMDMat); headParams.worldHeadOrientation = extractRotation(worldHMDMat); + + headParams.hipsMatrix = worldToRig * myAvatar->getSensorToWorldMatrix() * myAvatar->deriveBodyFromHMDSensor(); + headParams.hipsEnabled = true; } else { + headParams.hipsEnabled = false; headParams.isInHMD = false; // We don't have a valid localHeadPosition. diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 2c9376d591..a27fd01b3c 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -87,6 +87,7 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: // build a list of valid targets from _targetVarVec and animVars _maxTargetIndex = -1; bool removeUnfoundJoints = false; + for (auto& targetVar : _targetVarVec) { if (targetVar.jointIndex == -1) { // this targetVar hasn't been validated yet... @@ -105,9 +106,8 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: 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()); - if (target.getType() == IKTarget::Type::HipsRelativeRotationAndPosition) { - translation += _hipsOffset; - } + AnimPose absPose(glm::vec3(1.0f), rotation, translation); + target.setPose(rotation, translation); target.setIndex(targetVar.jointIndex); targets.push_back(target); @@ -441,26 +441,6 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars computeTargets(animVars, targets, underPoses); } - // debug render ik targets - if (context.getEnableDebugDrawIKTargets()) { - const vec4 WHITE(1.0f); - glm::mat4 rigToAvatarMat = createMatFromQuatAndPos(Quaternions::Y_180, glm::vec3()); - - for (auto& target : targets) { - glm::mat4 geomTargetMat = createMatFromQuatAndPos(target.getRotation(), target.getTranslation()); - glm::mat4 avatarTargetMat = rigToAvatarMat * context.getGeometryToRigMatrix() * geomTargetMat; - - QString name = QString("ikTarget%1").arg(target.getIndex()); - DebugDraw::getInstance().addMyAvatarMarker(name, glmExtractRotation(avatarTargetMat), extractTranslation(avatarTargetMat), WHITE); - } - } else if (context.getEnableDebugDrawIKTargets() != _previousEnableDebugIKTargets) { - // remove markers if they were added last frame. - for (auto& target : targets) { - QString name = QString("ikTarget%1").arg(target.getIndex()); - DebugDraw::getInstance().removeMyAvatarMarker(name); - } - } - _previousEnableDebugIKTargets = context.getEnableDebugDrawIKTargets(); if (targets.empty()) { @@ -478,25 +458,52 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars { PROFILE_RANGE_EX(simulation_animation, "ik/shiftHips", 0xffff00ff, 0); - // shift hips according to the _hipsOffset from the previous frame - float offsetLength = glm::length(_hipsOffset); - const float MIN_HIPS_OFFSET_LENGTH = 0.03f; - if (offsetLength > MIN_HIPS_OFFSET_LENGTH && _hipsIndex >= 0) { - // but only if offset is long enough - float scaleFactor = ((offsetLength - MIN_HIPS_OFFSET_LENGTH) / offsetLength); - if (_hipsParentIndex == -1) { - // the hips are the root so _hipsOffset is in the correct frame - _relativePoses[_hipsIndex].trans() = underPoses[_hipsIndex].trans() + scaleFactor * _hipsOffset; - } else { - // the hips are NOT the root so we need to transform _hipsOffset into hips local-frame - glm::quat hipsFrameRotation = _relativePoses[_hipsParentIndex].rot(); - int index = _skeleton->getParentIndex(_hipsParentIndex); - while (index != -1) { - hipsFrameRotation *= _relativePoses[index].rot(); - index = _skeleton->getParentIndex(index); + // AJT: TODO only need abs poses below hips. + AnimPoseVec absolutePoses; + absolutePoses.resize(_relativePoses.size()); + computeAbsolutePoses(absolutePoses); + + for (auto& target: targets) { + if (target.getType() == IKTarget::Type::RotationAndPosition && target.getIndex() == _hipsIndex) { + AnimPose absPose = target.getPose(); + int parentIndex = _skeleton->getParentIndex(target.getIndex()); + if (parentIndex != -1) { + _relativePoses[_hipsIndex] = absolutePoses[parentIndex].inverse() * absPose; + } else { + _relativePoses[_hipsIndex] = absPose; } - _relativePoses[_hipsIndex].trans() = underPoses[_hipsIndex].trans() - + glm::inverse(glm::normalize(hipsFrameRotation)) * (scaleFactor * _hipsOffset); + } + } + + for (auto& target: targets) { + if (target.getType() == IKTarget::Type::HipsRelativeRotationAndPosition) { + AnimPose pose = target.getPose(); + pose.trans() = pose.trans() + (_relativePoses[_hipsIndex].trans() - underPoses[_hipsIndex].trans()); + target.setPose(pose.rot(), pose.trans()); + } + } + } + + { + PROFILE_RANGE_EX(simulation_animation, "ik/debugDraw", 0xffff00ff, 0); + + // debug render ik targets + if (context.getEnableDebugDrawIKTargets()) { + const vec4 WHITE(1.0f); + glm::mat4 rigToAvatarMat = createMatFromQuatAndPos(Quaternions::Y_180, glm::vec3()); + + for (auto& target : targets) { + glm::mat4 geomTargetMat = createMatFromQuatAndPos(target.getRotation(), target.getTranslation()); + glm::mat4 avatarTargetMat = rigToAvatarMat * context.getGeometryToRigMatrix() * geomTargetMat; + + QString name = QString("ikTarget%1").arg(target.getIndex()); + DebugDraw::getInstance().addMyAvatarMarker(name, glmExtractRotation(avatarTargetMat), extractTranslation(avatarTargetMat), WHITE); + } + } else if (context.getEnableDebugDrawIKTargets() != _previousEnableDebugIKTargets) { + // remove markers if they were added last frame. + for (auto& target : targets) { + QString name = QString("ikTarget%1").arg(target.getIndex()); + DebugDraw::getInstance().removeMyAvatarMarker(name); } } } @@ -505,60 +512,6 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars PROFILE_RANGE_EX(simulation_animation, "ik/ccd", 0xffff00ff, 0); solveWithCyclicCoordinateDescent(targets); } - - { - PROFILE_RANGE_EX(simulation_animation, "ik/measureHipsOffset", 0xffff00ff, 0); - - // measure new _hipsOffset for next frame - // by looking for discrepancies between where a targeted endEffector is - // and where it wants to be (after IK solutions are done) - glm::vec3 newHipsOffset = Vectors::ZERO; - for (auto& target: targets) { - int targetIndex = target.getIndex(); - if (targetIndex == _headIndex && _headIndex != -1) { - // special handling for headTarget - if (target.getType() == IKTarget::Type::RotationOnly) { - // we want to shift the hips to bring the underPose closer - // to where the head happens to be (overpose) - glm::vec3 under = _skeleton->getAbsolutePose(_headIndex, underPoses).trans(); - glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans(); - const float HEAD_OFFSET_SLAVE_FACTOR = 0.65f; - newHipsOffset += HEAD_OFFSET_SLAVE_FACTOR * (actual - under); - } else if (target.getType() == IKTarget::Type::HmdHead) { - // we want to shift the hips to bring the head to its designated position - glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans(); - _hipsOffset += target.getTranslation() - actual; - // and ignore all other targets - newHipsOffset = _hipsOffset; - break; - } else if (target.getType() == IKTarget::Type::RotationAndPosition) { - glm::vec3 actualPosition = _skeleton->getAbsolutePose(targetIndex, _relativePoses).trans(); - glm::vec3 targetPosition = target.getTranslation(); - newHipsOffset += targetPosition - actualPosition; - - // Add downward pressure on the hips - newHipsOffset *= 0.95f; - newHipsOffset -= 1.0f; - } - } else if (target.getType() == IKTarget::Type::RotationAndPosition) { - glm::vec3 actualPosition = _skeleton->getAbsolutePose(targetIndex, _relativePoses).trans(); - glm::vec3 targetPosition = target.getTranslation(); - newHipsOffset += targetPosition - actualPosition; - } - } - - // smooth transitions by relaxing _hipsOffset toward the new value - const float HIPS_OFFSET_SLAVE_TIMESCALE = 0.10f; - float tau = dt < HIPS_OFFSET_SLAVE_TIMESCALE ? dt / HIPS_OFFSET_SLAVE_TIMESCALE : 1.0f; - _hipsOffset += (newHipsOffset - _hipsOffset) * tau; - - // clamp the hips offset - float hipsOffsetLength = glm::length(_hipsOffset); - if (hipsOffsetLength > _maxHipsOffsetLength) { - _hipsOffset *= _maxHipsOffsetLength / hipsOffsetLength; - } - - } } } return _relativePoses; diff --git a/libraries/animation/src/AnimManipulator.cpp b/libraries/animation/src/AnimManipulator.cpp index 111501898a..070949ab3b 100644 --- a/libraries/animation/src/AnimManipulator.cpp +++ b/libraries/animation/src/AnimManipulator.cpp @@ -12,6 +12,16 @@ #include "AnimUtil.h" #include "AnimationLogging.h" +AnimManipulator::JointVar::JointVar(const QString& jointNameIn, Type rotationTypeIn, Type translationTypeIn, + const QString& rotationVarIn, const QString& translationVarIn) : + jointName(jointNameIn), + rotationType(rotationTypeIn), + translationType(translationTypeIn), + rotationVar(rotationVarIn), + translationVar(translationVarIn), + jointIndex(-1), + hasPerformedJointLookup(false) {} + AnimManipulator::AnimManipulator(const QString& id, float alpha) : AnimNode(AnimNode::Type::Manipulator, id), _alpha(alpha) { @@ -36,7 +46,10 @@ const AnimPoseVec& AnimManipulator::overlay(const AnimVariantMap& animVars, cons } for (auto& jointVar : _jointVars) { + if (!jointVar.hasPerformedJointLookup) { + + // map from joint name to joint index and cache the result. jointVar.jointIndex = _skeleton->nameToJointIndex(jointVar.jointName); if (jointVar.jointIndex < 0) { qCWarning(animation) << "AnimManipulator could not find jointName" << jointVar.jointName << "in skeleton"; @@ -100,34 +113,62 @@ AnimPose AnimManipulator::computeRelativePoseFromJointVar(const AnimVariantMap& AnimPose defaultAbsPose = _skeleton->getAbsolutePose(jointVar.jointIndex, underPoses); - if (jointVar.type == JointVar::Type::AbsoluteRotation || jointVar.type == JointVar::Type::AbsolutePosition) { + // compute relative translation + glm::vec3 relTrans; + switch (jointVar.translationType) { + case JointVar::Type::Absolute: { + glm::vec3 absTrans = animVars.lookupRigToGeometry(jointVar.translationVar, defaultAbsPose.trans()); - if (jointVar.type == JointVar::Type::AbsoluteRotation) { - defaultAbsPose.rot() = animVars.lookupRigToGeometry(jointVar.var, defaultAbsPose.rot()); - } else if (jointVar.type == JointVar::Type::AbsolutePosition) { - defaultAbsPose.trans() = animVars.lookupRigToGeometry(jointVar.var, defaultAbsPose.trans()); + // convert to from absolute to relative. + AnimPose parentAbsPose; + int parentIndex = _skeleton->getParentIndex(jointVar.jointIndex); + if (parentIndex >= 0) { + parentAbsPose = _skeleton->getAbsolutePose(parentIndex, underPoses); + } + + // convert from absolute to relative + relTrans = transformPoint(parentAbsPose.inverse(), absTrans); + break; } - - // because jointVar is absolute, we must use an absolute parent frame to convert into a relative pose. - AnimPose parentAbsPose = AnimPose::identity; - int parentIndex = _skeleton->getParentIndex(jointVar.jointIndex); - if (parentIndex >= 0) { - parentAbsPose = _skeleton->getAbsolutePose(parentIndex, underPoses); - } - - // convert from absolute to relative - return parentAbsPose.inverse() * defaultAbsPose; - - } else { - - // override the default rel pose - AnimPose relPose = defaultRelPose; - if (jointVar.type == JointVar::Type::RelativeRotation) { - relPose.rot() = animVars.lookupRigToGeometry(jointVar.var, defaultRelPose.rot()); - } else if (jointVar.type == JointVar::Type::RelativePosition) { - relPose.trans() = animVars.lookupRigToGeometry(jointVar.var, defaultRelPose.trans()); - } - - return relPose; + case JointVar::Type::Relative: + relTrans = animVars.lookupRigToGeometryVector(jointVar.translationVar, defaultRelPose.trans()); + break; + case JointVar::Type::UnderPose: + relTrans = underPoses[jointVar.jointIndex].trans(); + break; + case JointVar::Type::Default: + default: + relTrans = defaultRelPose.trans(); + break; } + + glm::quat relRot; + switch (jointVar.rotationType) { + case JointVar::Type::Absolute: { + glm::quat absRot = animVars.lookupRigToGeometry(jointVar.translationVar, defaultAbsPose.rot()); + + // convert to from absolute to relative. + AnimPose parentAbsPose; + int parentIndex = _skeleton->getParentIndex(jointVar.jointIndex); + if (parentIndex >= 0) { + parentAbsPose = _skeleton->getAbsolutePose(parentIndex, underPoses); + } + + // convert from absolute to relative + relRot = glm::inverse(parentAbsPose.rot()) * absRot; + break; + } + case JointVar::Type::Relative: + relRot = animVars.lookupRigToGeometry(jointVar.translationVar, defaultRelPose.rot()); + break; + case JointVar::Type::UnderPose: + relRot = underPoses[jointVar.jointIndex].rot(); + break; + case JointVar::Type::Default: + default: + relRot = defaultRelPose.rot(); + break; + } + + return AnimPose(glm::vec3(1), relRot, relTrans); } diff --git a/libraries/animation/src/AnimManipulator.h b/libraries/animation/src/AnimManipulator.h index 26f50a7dd9..1134f75da9 100644 --- a/libraries/animation/src/AnimManipulator.h +++ b/libraries/animation/src/AnimManipulator.h @@ -31,17 +31,20 @@ public: struct JointVar { enum class Type { - AbsoluteRotation = 0, - AbsolutePosition, - RelativeRotation, - RelativePosition, + Absolute, + Relative, + UnderPose, + Default, NumTypes }; - JointVar(const QString& varIn, const QString& jointNameIn, Type typeIn) : var(varIn), jointName(jointNameIn), type(typeIn), jointIndex(-1), hasPerformedJointLookup(false) {} - QString var = ""; + JointVar(const QString& jointNameIn, Type rotationType, Type translationType, const QString& rotationVarIn, const QString& translationVarIn); QString jointName = ""; - Type type = Type::AbsoluteRotation; + Type rotationType = Type::Absolute; + Type translationType = Type::Absolute; + QString rotationVar = ""; + QString translationVar = ""; + int jointIndex = -1; bool hasPerformedJointLookup = false; bool isRelative = false; diff --git a/libraries/animation/src/AnimNodeLoader.cpp b/libraries/animation/src/AnimNodeLoader.cpp index 876913fc58..bda4541f36 100644 --- a/libraries/animation/src/AnimNodeLoader.cpp +++ b/libraries/animation/src/AnimNodeLoader.cpp @@ -79,10 +79,10 @@ static AnimStateMachine::InterpType stringToInterpType(const QString& str) { static const char* animManipulatorJointVarTypeToString(AnimManipulator::JointVar::Type type) { switch (type) { - case AnimManipulator::JointVar::Type::AbsoluteRotation: return "absoluteRotation"; - case AnimManipulator::JointVar::Type::AbsolutePosition: return "absolutePosition"; - case AnimManipulator::JointVar::Type::RelativeRotation: return "relativeRotation"; - case AnimManipulator::JointVar::Type::RelativePosition: return "relativePosition"; + case AnimManipulator::JointVar::Type::Absolute: return "absolute"; + case AnimManipulator::JointVar::Type::Relative: return "relative"; + case AnimManipulator::JointVar::Type::UnderPose: return "underPose"; + case AnimManipulator::JointVar::Type::Default: return "default"; case AnimManipulator::JointVar::Type::NumTypes: return nullptr; }; return nullptr; @@ -339,7 +339,8 @@ static const char* boneSetStrings[AnimOverlay::NumBoneSets] = { "spineOnly", "empty", "leftHand", - "rightHand" + "rightHand", + "hipsOnly" }; static AnimOverlay::BoneSet stringToBoneSetEnum(const QString& str) { @@ -406,17 +407,25 @@ static AnimNode::Pointer loadManipulatorNode(const QJsonObject& jsonObj, const Q } auto jointObj = jointValue.toObject(); - READ_STRING(type, jointObj, id, jsonUrl, nullptr); READ_STRING(jointName, jointObj, id, jsonUrl, nullptr); - READ_STRING(var, jointObj, id, jsonUrl, nullptr); + READ_STRING(rotationType, jointObj, id, jsonUrl, nullptr); + READ_STRING(translationType, jointObj, id, jsonUrl, nullptr); + READ_STRING(rotationVar, jointObj, id, jsonUrl, nullptr); + READ_STRING(translationVar, jointObj, id, jsonUrl, nullptr); - AnimManipulator::JointVar::Type jointVarType = stringToAnimManipulatorJointVarType(type); - if (jointVarType == AnimManipulator::JointVar::Type::NumTypes) { - qCCritical(animation) << "AnimNodeLoader, bad type in \"joints\", id =" << id << ", url =" << jsonUrl.toDisplayString(); - return nullptr; + AnimManipulator::JointVar::Type jointVarRotationType = stringToAnimManipulatorJointVarType(rotationType); + if (jointVarRotationType == AnimManipulator::JointVar::Type::NumTypes) { + qCWarning(animation) << "AnimNodeLoader, bad rotationType in \"joints\", id =" << id << ", url =" << jsonUrl.toDisplayString(); + jointVarRotationType = AnimManipulator::JointVar::Type::Default; } - AnimManipulator::JointVar jointVar(var, jointName, jointVarType); + AnimManipulator::JointVar::Type jointVarTranslationType = stringToAnimManipulatorJointVarType(translationType); + if (jointVarTranslationType == AnimManipulator::JointVar::Type::NumTypes) { + qCWarning(animation) << "AnimNodeLoader, bad translationType in \"joints\", id =" << id << ", url =" << jsonUrl.toDisplayString(); + jointVarTranslationType = AnimManipulator::JointVar::Type::Default; + } + + AnimManipulator::JointVar jointVar(jointName, jointVarRotationType, jointVarTranslationType, rotationVar, translationVar); node->addJointVar(jointVar); }; diff --git a/libraries/animation/src/AnimOverlay.cpp b/libraries/animation/src/AnimOverlay.cpp index dbc635af66..e086413dde 100644 --- a/libraries/animation/src/AnimOverlay.cpp +++ b/libraries/animation/src/AnimOverlay.cpp @@ -34,6 +34,7 @@ void AnimOverlay::buildBoneSet(BoneSet boneSet) { case SpineOnlyBoneSet: buildSpineOnlyBoneSet(); break; case LeftHandBoneSet: buildLeftHandBoneSet(); break; case RightHandBoneSet: buildRightHandBoneSet(); break; + case HipsOnlyBoneSet: buildHipsOnlyBoneSet(); break; default: case EmptyBoneSet: buildEmptyBoneSet(); break; } @@ -188,6 +189,13 @@ void AnimOverlay::buildRightHandBoneSet() { }); } +void AnimOverlay::buildHipsOnlyBoneSet() { + assert(_skeleton); + buildEmptyBoneSet(); + int hipsJoint = _skeleton->nameToJointIndex("Hips"); + _boneSetVec[hipsJoint] = 1.0f; +} + // for AnimDebugDraw rendering const AnimPoseVec& AnimOverlay::getPosesInternal() const { return _poses; diff --git a/libraries/animation/src/AnimOverlay.h b/libraries/animation/src/AnimOverlay.h index 2f34c07309..ed9439feb7 100644 --- a/libraries/animation/src/AnimOverlay.h +++ b/libraries/animation/src/AnimOverlay.h @@ -37,6 +37,7 @@ public: EmptyBoneSet, LeftHandBoneSet, RightHandBoneSet, + HipsOnlyBoneSet, NumBoneSets }; @@ -75,6 +76,7 @@ public: void buildEmptyBoneSet(); void buildLeftHandBoneSet(); void buildRightHandBoneSet(); + void buildHipsOnlyBoneSet(); // no copies AnimOverlay(const AnimOverlay&) = delete; diff --git a/libraries/animation/src/AnimVariant.h b/libraries/animation/src/AnimVariant.h index 3466013ff6..d383b5abb8 100644 --- a/libraries/animation/src/AnimVariant.h +++ b/libraries/animation/src/AnimVariant.h @@ -165,6 +165,15 @@ public: } } + glm::vec3 lookupRigToGeometryVector(const QString& key, const glm::vec3& defaultValue) const { + if (key.isEmpty()) { + return defaultValue; + } else { + auto iter = _map.find(key); + return iter != _map.end() ? transformVectorFast(_rigToGeometryMat, iter->second.getVec3()) : defaultValue; + } + } + const glm::quat& lookupRaw(const QString& key, const glm::quat& defaultValue) const { if (key.isEmpty()) { return defaultValue; diff --git a/libraries/animation/src/IKTarget.h b/libraries/animation/src/IKTarget.h index 9ea34a6165..acb01d9861 100644 --- a/libraries/animation/src/IKTarget.h +++ b/libraries/animation/src/IKTarget.h @@ -21,13 +21,14 @@ public: RotationOnly, HmdHead, HipsRelativeRotationAndPosition, - Unknown, + Unknown }; IKTarget() {} const glm::vec3& getTranslation() const { return _pose.trans(); } const glm::quat& getRotation() const { return _pose.rot(); } + const AnimPose& getPose() const { return _pose; } int getIndex() const { return _index; } Type getType() const { return _type; } diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index fb0867e2de..adc40d6e81 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1024,6 +1024,15 @@ void Rig::updateFromHeadParameters(const HeadParameters& params, float dt) { _animVars.set("isTalking", params.isTalking); _animVars.set("notIsTalking", !params.isTalking); + + // AJT: + if (params.hipsEnabled) { + _animVars.set("hipsType", (int)IKTarget::Type::RotationAndPosition); + _animVars.set("hipsPosition", extractTranslation(params.hipsMatrix)); + _animVars.set("hipsRotation", glmExtractRotation(params.hipsMatrix) * Quaternions::Y_180); + } else { + _animVars.set("hipsType", (int)IKTarget::Type::Unknown); + } } void Rig::updateFromEyeParameters(const EyeParameters& params) { @@ -1094,7 +1103,7 @@ void Rig::updateNeckJoint(int index, const HeadParameters& params) { _animVars.set("headPosition", headPos); _animVars.set("headRotation", headRot); - _animVars.set("headType", (int)IKTarget::Type::HmdHead); + _animVars.set("headType", (int)IKTarget::Type::RotationAndPosition); _animVars.set("neckPosition", neckPos); _animVars.set("neckRotation", neckRot); _animVars.set("neckType", (int)IKTarget::Type::Unknown); // 'Unknown' disables the target diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 2cd20c2704..89f0d624f9 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -45,6 +45,8 @@ public: glm::quat worldHeadOrientation = glm::quat(); // world space (-z forward) glm::quat rigHeadOrientation = glm::quat(); // rig space (-z forward) glm::vec3 rigHeadPosition = glm::vec3(); // rig space + glm::mat4 hipsMatrix = glm::mat4(); // rig space + bool hipsEnabled = false; bool isInHMD = false; int neckJointIndex = -1; bool isTalking = false; From adaf7dda7c3b1105f8d93251691aa75a497a702c Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 7 Apr 2017 17:47:53 -0700 Subject: [PATCH 02/17] Check in viveMotionCapture test script. --- .../animation/src/AnimInverseKinematics.cpp | 4 + scripts/developer/tests/viveMotionCapture.js | 198 ++++++++++++++++++ 2 files changed, 202 insertions(+) create mode 100644 scripts/developer/tests/viveMotionCapture.js diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index a27fd01b3c..7fda318f5c 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -277,6 +277,9 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe const float MIN_AXIS_LENGTH = 1.0e-4f; RotationConstraint* constraint = getConstraint(pivotIndex); + + // AJT: disabled special case for the lower spine. + /* if (constraint && constraint->isLowerSpine() && tipIndex != _headIndex) { // for these types of targets we only allow twist at the lower-spine // (this prevents the hand targets from bending the spine too much and thereby driving the hips too far) @@ -292,6 +295,7 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe targetLine = Vectors::ZERO; } } + */ glm::vec3 axis = glm::cross(leverArm, targetLine); float axisLength = glm::length(axis); diff --git a/scripts/developer/tests/viveMotionCapture.js b/scripts/developer/tests/viveMotionCapture.js new file mode 100644 index 0000000000..0e6119a714 --- /dev/null +++ b/scripts/developer/tests/viveMotionCapture.js @@ -0,0 +1,198 @@ +/* global Xform */ +Script.include("/~/system/libraries/Xform.js"); + +var TRACKED_OBJECT_POSES = [ + "TrackedObject00", "TrackedObject01", "TrackedObject02", "TrackedObject03", + "TrackedObject04", "TrackedObject05", "TrackedObject06", "TrackedObject07", + "TrackedObject08", "TrackedObject09", "TrackedObject10", "TrackedObject11", + "TrackedObject12", "TrackedObject13", "TrackedObject14", "TrackedObject15" +]; + +var calibrated = false; +var rightTriggerPressed = false; +var leftTriggerPressed = false; + +var MAPPING_NAME = "com.highfidelity.viveMotionCapture"; + +var mapping = Controller.newMapping(MAPPING_NAME); +mapping.from([Controller.Standard.RTClick]).peek().to(function (value) { + rightTriggerPressed = (value !== 0) ? true : false; +}); +mapping.from([Controller.Standard.LTClick]).peek().to(function (value) { + leftTriggerPressed = (value !== 0) ? true : false; +}); + +Controller.enableMapping(MAPPING_NAME); + +var leftFoot; +var rightFoot; +var hips; + +var Y_180 = {x: 0, y: 1, z: 0, w: 0}; + +function computeOffsetXform(pose, jointIndex) { + var poseXform = new Xform(pose.rotation, pose.translation); + var referenceXform = new Xform(MyAvatar.getAbsoluteDefaultJointRotationInObjectFrame(jointIndex), + MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(jointIndex)); + return Xform.mul(poseXform.inv(), referenceXform); +} + +function calibrate() { + print("AJT: calibrating"); + var poses = []; + if (Controller.Hardware.Vive) { + TRACKED_OBJECT_POSES.forEach(function (key) { + var channel = Controller.Hardware.Vive[key]; + var pose = Controller.getPoseValue(channel); + if (pose.valid) { + poses.push({ + channel: channel, + pose: pose + }); + } + }); + } + + if (poses.length >= 2) { + // sort by y + poses.sort(function(a, b) { + var ay = a.pose.translation.y; + var by = b.pose.translation.y; + return ay - by; + }); + + if (poses[0].pose.translation.x > poses[1].pose.translation.x) { + rightFoot = poses[0]; + leftFoot = poses[1]; + } else { + rightFoot = poses[1]; + leftFoot = poses[0]; + } + + // compute offsets + rightFoot.offsetXform = computeOffsetXform(rightFoot.pose, MyAvatar.getJointIndex("RightFoot")); + leftFoot.offsetXform = computeOffsetXform(leftFoot.pose, MyAvatar.getJointIndex("LeftFoot")); + + print("AJT: rightFoot = " + JSON.stringify(rightFoot)); + print("AJT: leftFoot = " + JSON.stringify(leftFoot)); + + if (poses.length >= 3) { + hips = poses[2]; + hips.offsetXform = computeOffsetXform(hips.pose, MyAvatar.getJointIndex("Hips")); + + print("AJT: hips = " + JSON.stringify(hips)); + } + } else { + print("AJT: could not find two trackers, try again!"); + } +} + +var ikTypes = { + RotationAndPosition: 0, + RotationOnly: 1, + HmdHead: 2, + HipsRelativeRotationAndPosition: 3, + Off: 4 +}; + +var handlerId; + +function update(dt) { + if (rightTriggerPressed && leftTriggerPressed) { + if (!calibrated) { + calibrate(); + calibrated = true; + + if (handlerId) { + MyAvatar.removeAnimationStateHandler(handlerId); + } + + // hook up anim state callback + var propList = [ + "leftFootType", "leftFootPosition", "leftFootRotation", + "rightFootType", "rightFootPosition", "rightFootRotation", + "hipsType", "hipsPosition", "hipsRotation" + ]; + + handlerId = MyAvatar.addAnimationStateHandler(function (props) { + + var result = {}, pose, offsetXform, xform; + if (rightFoot) { + pose = Controller.getPoseValue(rightFoot.channel); + offsetXform = rightFoot.offsetXform; + + xform = Xform.mul(new Xform(pose.rotation, pose.translation), offsetXform); + result.rightFootType = ikTypes.RotationAndPosition; + result.rightFootPosition = Vec3.multiplyQbyV(Y_180, xform.pos); + result.rightFootRotation = Quat.multiply(Y_180, xform.rot); + + } else { + result.rightFootType = props.rightFootType; + result.rightFootPositon = props.rightFootPosition; + result.rightFootRotation = props.rightFootRotation; + } + + if (leftFoot) { + pose = Controller.getPoseValue(leftFoot.channel); + offsetXform = leftFoot.offsetXform; + xform = Xform.mul(new Xform(pose.rotation, pose.translation), offsetXform); + result.leftFootType = ikTypes.RotationAndPosition; + result.leftFootPosition = Vec3.multiplyQbyV(Y_180, xform.pos); + result.leftFootRotation = Quat.multiply(Y_180, xform.rot); + } else { + result.leftFootType = props.leftFootType; + result.leftFootPositon = props.leftFootPosition; + result.leftFootRotation = props.leftFootRotation; + } + + if (hips) { + pose = Controller.getPoseValue(hips.channel); + offsetXform = hips.offsetXform; + xform = Xform.mul(new Xform(pose.rotation, pose.translation), offsetXform); + result.hipsType = ikTypes.RotationAndPosition; + result.hipsPosition = Vec3.multiplyQbyV(Y_180, xform.pos); + result.hipsRotation = Quat.multiply(Y_180, xform.rot); + } else { + result.hipsType = props.hipsType; + result.hipsPositon = props.hipsPosition; + result.hipsRotation = props.hipsRotation; + } + + return result; + }, propList); + + } + } else { + calibrated = false; + } + + var drawMarkers = false; + if (drawMarkers) { + var RED = {x: 1, y: 0, z: 0, w: 1}; + var GREEN = {x: 0, y: 1, z: 0, w: 1}; + var BLUE = {x: 0, y: 0, z: 1, w: 1}; + + if (leftFoot) { + var leftFootPose = Controller.getPoseValue(leftFoot.channel); + DebugDraw.addMyAvatarMarker("leftFootTracker", leftFootPose.rotation, leftFootPose.translation, BLUE); + } + + if (rightFoot) { + var rightFootPose = Controller.getPoseValue(rightFoot.channel); + DebugDraw.addMyAvatarMarker("rightFootTracker", rightFootPose.rotation, rightFootPose.translation, RED); + } + + if (hips) { + var hipsPose = Controller.getPoseValue(hips.channel); + DebugDraw.addMyAvatarMarker("hipsTracker", hipsPose.rotation, hipsPose.translation, GREEN); + } + } +} + +Script.update.connect(update); + +Script.scriptEnding.connect(function () { + Controller.disableMapping(MAPPING_NAME); + Script.update.disconnect(update); +}); +var TRIGGER_OFF_VALUE = 0.1; From 0ebaba7cf890aa7544365cff5057c87a87642e33 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 10 Apr 2017 13:36:30 -0700 Subject: [PATCH 03/17] Now supports sensorConfig with hips and chest sensors --- .../resources/avatar/avatar-animation.json | 8 +- libraries/animation/src/Rig.cpp | 3 + scripts/developer/tests/viveMotionCapture.js | 123 +++++++++++++----- scripts/developer/tests/viveTrackedObjects.js | 4 +- 4 files changed, 100 insertions(+), 38 deletions(-) diff --git a/interface/resources/avatar/avatar-animation.json b/interface/resources/avatar/avatar-animation.json index daa69b95f7..9efe3dd29b 100644 --- a/interface/resources/avatar/avatar-animation.json +++ b/interface/resources/avatar/avatar-animation.json @@ -81,10 +81,10 @@ "typeVar": "leftFootType" }, { - "jointName": "Neck", - "positionVar": "neckPosition", - "rotationVar": "neckRotation", - "typeVar": "neckType" + "jointName": "Spine2", + "positionVar": "spine2Position", + "rotationVar": "spine2Rotation", + "typeVar": "spine2Type" }, { "jointName": "Head", diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index adc40d6e81..c8717fd11e 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1033,6 +1033,9 @@ void Rig::updateFromHeadParameters(const HeadParameters& params, float dt) { } else { _animVars.set("hipsType", (int)IKTarget::Type::Unknown); } + + // by default this IK target is disabled. + _animVars.set("spine2Type", (int)IKTarget::Type::Unknown); } void Rig::updateFromEyeParameters(const EyeParameters& params) { diff --git a/scripts/developer/tests/viveMotionCapture.js b/scripts/developer/tests/viveMotionCapture.js index 0e6119a714..27c6809f5c 100644 --- a/scripts/developer/tests/viveMotionCapture.js +++ b/scripts/developer/tests/viveMotionCapture.js @@ -27,18 +27,59 @@ Controller.enableMapping(MAPPING_NAME); var leftFoot; var rightFoot; var hips; +var spine2; + +var FEET_ONLY = 0; +var FEET_AND_HIPS = 1; +var FEET_AND_CHEST = 2; +var FEET_HIPS_AND_CHEST = 3; + +var SENSOR_CONFIG_NAMES = [ + "FeetOnly", + "FeetAndHips", + "FeetAndChest", + "FeetHipsAndChest" +]; + +var ANIM_VARS = [ + "leftFootType", + "leftFootPosition", + "leftFootRotation", + "rightFootType", + "rightFootPosition", + "rightFootRotation", + "hipsType", + "hipsPosition", + "hipsRotation", + "spine2Type", + "spine2Position", + "spine2Rotation" +]; + +var sensorConfig = FEET_HIPS_AND_CHEST; var Y_180 = {x: 0, y: 1, z: 0, w: 0}; +var Y_180_XFORM = new Xform(Y_180, {x: 0, y: 0, z: 0}); function computeOffsetXform(pose, jointIndex) { var poseXform = new Xform(pose.rotation, pose.translation); + + // TODO: we can do better here... + // one idea, hang default pose skeleton from HMD head. var referenceXform = new Xform(MyAvatar.getAbsoluteDefaultJointRotationInObjectFrame(jointIndex), MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(jointIndex)); + return Xform.mul(poseXform.inv(), referenceXform); } function calibrate() { - print("AJT: calibrating"); + print("AJT: calibrating..."); + + leftFoot = undefined; + rightFoot = undefined; + hips = undefined; + spine2 = undefined; + var poses = []; if (Controller.Hardware.Vive) { TRACKED_OBJECT_POSES.forEach(function (key) { @@ -76,12 +117,30 @@ function calibrate() { print("AJT: rightFoot = " + JSON.stringify(rightFoot)); print("AJT: leftFoot = " + JSON.stringify(leftFoot)); - if (poses.length >= 3) { + if (sensorConfig === FEET_ONLY) { + // we're done! + } else if (sensorConfig === FEET_AND_HIPS && poses.length >= 3) { hips = poses[2]; - hips.offsetXform = computeOffsetXform(hips.pose, MyAvatar.getJointIndex("Hips")); + } else if (sensorConfig === FEET_AND_CHEST && poses.length >= 3) { + spine2 = poses[2]; + } else if (sensorConfig === FEET_HIPS_AND_CHEST && poses.length >= 4) { + hips = poses[2]; + spine2 = poses[3]; + } else { + // TODO: better error messages + print("AJT: could not calibrate for sensor config " + SENSOR_CONFIG_NAMES[sensorConfig] + ", please try again!"); + } + if (hips) { + hips.offsetXform = computeOffsetXform(hips.pose, MyAvatar.getJointIndex("Hips")); print("AJT: hips = " + JSON.stringify(hips)); } + + if (spine2) { + spine2.offsetXform = computeOffsetXform(spine2.pose, MyAvatar.getJointIndex("Spine2")); + print("AJT: spine2 = " + JSON.stringify(spine2)); + } + } else { print("AJT: could not find two trackers, try again!"); } @@ -97,6 +156,12 @@ var ikTypes = { var handlerId; +function computeIKTargetXform(jointInfo) { + var pose = Controller.getPoseValue(jointInfo.channel); + var offsetXform = jointInfo.offsetXform; + return Xform.mul(Y_180_XFORM, Xform.mul(new Xform(pose.rotation, pose.translation), offsetXform)); +} + function update(dt) { if (rightTriggerPressed && leftTriggerPressed) { if (!calibrated) { @@ -107,59 +172,53 @@ function update(dt) { MyAvatar.removeAnimationStateHandler(handlerId); } - // hook up anim state callback - var propList = [ - "leftFootType", "leftFootPosition", "leftFootRotation", - "rightFootType", "rightFootPosition", "rightFootRotation", - "hipsType", "hipsPosition", "hipsRotation" - ]; - handlerId = MyAvatar.addAnimationStateHandler(function (props) { - var result = {}, pose, offsetXform, xform; + var result = {}, xform; if (rightFoot) { - pose = Controller.getPoseValue(rightFoot.channel); - offsetXform = rightFoot.offsetXform; - - xform = Xform.mul(new Xform(pose.rotation, pose.translation), offsetXform); + xform = computeIKTargetXform(rightFoot); result.rightFootType = ikTypes.RotationAndPosition; - result.rightFootPosition = Vec3.multiplyQbyV(Y_180, xform.pos); - result.rightFootRotation = Quat.multiply(Y_180, xform.rot); - + result.rightFootPosition = xform.pos; + result.rightFootRotation = xform.rot; } else { result.rightFootType = props.rightFootType; - result.rightFootPositon = props.rightFootPosition; + result.rightFootPosition = props.rightFootPosition; result.rightFootRotation = props.rightFootRotation; } if (leftFoot) { - pose = Controller.getPoseValue(leftFoot.channel); - offsetXform = leftFoot.offsetXform; - xform = Xform.mul(new Xform(pose.rotation, pose.translation), offsetXform); + xform = computeIKTargetXform(leftFoot); result.leftFootType = ikTypes.RotationAndPosition; - result.leftFootPosition = Vec3.multiplyQbyV(Y_180, xform.pos); - result.leftFootRotation = Quat.multiply(Y_180, xform.rot); + result.leftFootPosition = xform.pos; + result.leftFootRotation = xform.rot; } else { result.leftFootType = props.leftFootType; - result.leftFootPositon = props.leftFootPosition; + result.leftFootPosition = props.leftFootPosition; result.leftFootRotation = props.leftFootRotation; } if (hips) { - pose = Controller.getPoseValue(hips.channel); - offsetXform = hips.offsetXform; - xform = Xform.mul(new Xform(pose.rotation, pose.translation), offsetXform); + xform = computeIKTargetXform(hips); result.hipsType = ikTypes.RotationAndPosition; - result.hipsPosition = Vec3.multiplyQbyV(Y_180, xform.pos); - result.hipsRotation = Quat.multiply(Y_180, xform.rot); + result.hipsPosition = xform.pos; + result.hipsRotation = xform.rot; } else { result.hipsType = props.hipsType; - result.hipsPositon = props.hipsPosition; + result.hipsPosition = props.hipsPosition; result.hipsRotation = props.hipsRotation; } + if (spine2) { + xform = computeIKTargetXform(spine2); + result.spine2Type = ikTypes.RotationAndPosition; + result.spine2Position = xform.pos; + result.spine2Rotation = xform.rot; + } else { + result.spine2Type = ikTypes.Off; + } + return result; - }, propList); + }, ANIM_VARS); } } else { diff --git a/scripts/developer/tests/viveTrackedObjects.js b/scripts/developer/tests/viveTrackedObjects.js index 78911538e4..4155afb82b 100644 --- a/scripts/developer/tests/viveTrackedObjects.js +++ b/scripts/developer/tests/viveTrackedObjects.js @@ -18,14 +18,14 @@ function shutdown() { }); } -var WHITE = {x: 1, y: 1, z: 1, w: 1}; +var BLUE = {x: 0, y: 0, z: 1, w: 1}; function update(dt) { if (Controller.Hardware.Vive) { TRACKED_OBJECT_POSES.forEach(function (key) { var pose = Controller.getPoseValue(Controller.Hardware.Vive[key]); if (pose.valid) { - DebugDraw.addMyAvatarMarker(key, pose.rotation, pose.translation, WHITE); + DebugDraw.addMyAvatarMarker(key, pose.rotation, pose.translation, BLUE); } else { DebugDraw.removeMyAvatarMarker(key); } From d3b4a2c08d968da1ce0b2ef6a54926e350b10b5f Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 10 Apr 2017 15:08:03 -0700 Subject: [PATCH 04/17] better "auto" configuration --- libraries/script-engine/src/Quat.cpp | 7 ++ libraries/script-engine/src/Quat.h | 2 + scripts/developer/tests/viveMotionCapture.js | 86 ++++++++++++++++---- 3 files changed, 77 insertions(+), 18 deletions(-) diff --git a/libraries/script-engine/src/Quat.cpp b/libraries/script-engine/src/Quat.cpp index 6d49ed27c1..05002dcf5d 100644 --- a/libraries/script-engine/src/Quat.cpp +++ b/libraries/script-engine/src/Quat.cpp @@ -122,3 +122,10 @@ bool Quat::equal(const glm::quat& q1, const glm::quat& q2) { return q1 == q2; } +glm::quat Quat::cancelOutRollAndPitch(const glm::quat& q) { + return ::cancelOutRollAndPitch(q); +} + +glm::quat Quat::cancelOutRoll(const glm::quat& q) { + return ::cancelOutRoll(q); +} diff --git a/libraries/script-engine/src/Quat.h b/libraries/script-engine/src/Quat.h index 8a88767a41..ee3ab9aa7c 100644 --- a/libraries/script-engine/src/Quat.h +++ b/libraries/script-engine/src/Quat.h @@ -60,6 +60,8 @@ public slots: float dot(const glm::quat& q1, const glm::quat& q2); void print(const QString& label, const glm::quat& q); bool equal(const glm::quat& q1, const glm::quat& q2); + glm::quat cancelOutRollAndPitch(const glm::quat& q); + glm::quat cancelOutRoll(const glm::quat& q); }; #endif // hifi_Quat_h diff --git a/scripts/developer/tests/viveMotionCapture.js b/scripts/developer/tests/viveMotionCapture.js index 27c6809f5c..6cb0f92b9b 100644 --- a/scripts/developer/tests/viveMotionCapture.js +++ b/scripts/developer/tests/viveMotionCapture.js @@ -33,12 +33,14 @@ var FEET_ONLY = 0; var FEET_AND_HIPS = 1; var FEET_AND_CHEST = 2; var FEET_HIPS_AND_CHEST = 3; +var AUTO = 4; var SENSOR_CONFIG_NAMES = [ "FeetOnly", "FeetAndHips", "FeetAndChest", - "FeetHipsAndChest" + "FeetHipsAndChest", + "Auto" ]; var ANIM_VARS = [ @@ -56,30 +58,47 @@ var ANIM_VARS = [ "spine2Rotation" ]; -var sensorConfig = FEET_HIPS_AND_CHEST; +var sensorConfig = AUTO; var Y_180 = {x: 0, y: 1, z: 0, w: 0}; var Y_180_XFORM = new Xform(Y_180, {x: 0, y: 0, z: 0}); -function computeOffsetXform(pose, jointIndex) { +function computeOffsetXform(defaultToReferenceXform, pose, jointIndex) { var poseXform = new Xform(pose.rotation, pose.translation); - // TODO: we can do better here... - // one idea, hang default pose skeleton from HMD head. - var referenceXform = new Xform(MyAvatar.getAbsoluteDefaultJointRotationInObjectFrame(jointIndex), - MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(jointIndex)); + var defaultJointXform = new Xform(MyAvatar.getAbsoluteDefaultJointRotationInObjectFrame(jointIndex), + MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(jointIndex)); - return Xform.mul(poseXform.inv(), referenceXform); + var referenceJointXform = Xform.mul(defaultToReferenceXform, defaultJointXform); + + return Xform.mul(poseXform.inv(), referenceJointXform); +} + +function computeDefaultToReferenceXform() { + var headIndex = MyAvatar.getJointIndex("Head"); + if (headIndex >= 0) { + var defaultHeadXform = new Xform(MyAvatar.getAbsoluteDefaultJointRotationInObjectFrame(headIndex), + MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(headIndex)); + var currentHeadXform = new Xform(Quat.cancelOutRollAndPitch(MyAvatar.getAbsoluteJointRotationInObjectFrame(headIndex)), + MyAvatar.getAbsoluteJointTranslationInObjectFrame(headIndex)); + + var defaultToReferenceXform = Xform.mul(currentHeadXform, defaultHeadXform.inv()); + + return defaultToReferenceXform; + } else { + return new Xform.ident(); + } } function calibrate() { - print("AJT: calibrating..."); leftFoot = undefined; rightFoot = undefined; hips = undefined; spine2 = undefined; + var defaultToReferenceXform = computeDefaultToReferenceXform(); + var poses = []; if (Controller.Hardware.Vive) { TRACKED_OBJECT_POSES.forEach(function (key) { @@ -94,6 +113,23 @@ function calibrate() { }); } + print("AJT: calibrating, num tracked poses = " + poses.length + ", sensorConfig = " + SENSOR_CONFIG_NAMES[sensorConfig]); + + var config = sensorConfig; + + if (config === AUTO) { + if (poses.length === 2) { + config = FEET_ONLY; + } else if (poses.length === 3) { + config = FEET_AND_HIPS; + } else if (poses.length >= 4) { + config = FEET_HIPS_AND_CHEST; + } else { + print("AJT: auto config failed: poses.length = " + poses.length); + config = FEET_ONLY; + } + } + if (poses.length >= 2) { // sort by y poses.sort(function(a, b) { @@ -111,33 +147,33 @@ function calibrate() { } // compute offsets - rightFoot.offsetXform = computeOffsetXform(rightFoot.pose, MyAvatar.getJointIndex("RightFoot")); - leftFoot.offsetXform = computeOffsetXform(leftFoot.pose, MyAvatar.getJointIndex("LeftFoot")); + rightFoot.offsetXform = computeOffsetXform(defaultToReferenceXform, rightFoot.pose, MyAvatar.getJointIndex("RightFoot")); + leftFoot.offsetXform = computeOffsetXform(defaultToReferenceXform, leftFoot.pose, MyAvatar.getJointIndex("LeftFoot")); print("AJT: rightFoot = " + JSON.stringify(rightFoot)); print("AJT: leftFoot = " + JSON.stringify(leftFoot)); - if (sensorConfig === FEET_ONLY) { + if (config === FEET_ONLY) { // we're done! - } else if (sensorConfig === FEET_AND_HIPS && poses.length >= 3) { + } else if (config === FEET_AND_HIPS && poses.length >= 3) { hips = poses[2]; - } else if (sensorConfig === FEET_AND_CHEST && poses.length >= 3) { + } else if (config === FEET_AND_CHEST && poses.length >= 3) { spine2 = poses[2]; - } else if (sensorConfig === FEET_HIPS_AND_CHEST && poses.length >= 4) { + } else if (config === FEET_HIPS_AND_CHEST && poses.length >= 4) { hips = poses[2]; spine2 = poses[3]; } else { // TODO: better error messages - print("AJT: could not calibrate for sensor config " + SENSOR_CONFIG_NAMES[sensorConfig] + ", please try again!"); + print("AJT: could not calibrate for sensor config " + SENSOR_CONFIG_NAMES[config] + ", please try again!"); } if (hips) { - hips.offsetXform = computeOffsetXform(hips.pose, MyAvatar.getJointIndex("Hips")); + hips.offsetXform = computeOffsetXform(defaultToReferenceXform, hips.pose, MyAvatar.getJointIndex("Hips")); print("AJT: hips = " + JSON.stringify(hips)); } if (spine2) { - spine2.offsetXform = computeOffsetXform(spine2.pose, MyAvatar.getJointIndex("Spine2")); + spine2.offsetXform = computeOffsetXform(defaultToReferenceXform, spine2.pose, MyAvatar.getJointIndex("Spine2")); print("AJT: spine2 = " + JSON.stringify(spine2)); } @@ -246,6 +282,20 @@ function update(dt) { DebugDraw.addMyAvatarMarker("hipsTracker", hipsPose.rotation, hipsPose.translation, GREEN); } } + + var drawReferencePose = false; + if (drawReferencePose) { + var GREEN = {x: 0, y: 1, z: 0, w: 1}; + var defaultToReferenceXform = computeDefaultToReferenceXform(); + var leftFootIndex = MyAvatar.getJointIndex("LeftFoot"); + if (leftFootIndex > 0) { + var defaultLeftFootXform = new Xform(MyAvatar.getAbsoluteDefaultJointRotationInObjectFrame(leftFootIndex), + MyAvatar.getAbsoluteDefaultJointTranslationInObjectFrame(leftFootIndex)); + var referenceLeftFootXform = Xform.mul(defaultToReferenceXform, defaultLeftFootXform); + DebugDraw.addMyAvatarMarker("leftFootTracker", referenceLeftFootXform.rot, referenceLeftFootXform.pos, GREEN); + } + } + } Script.update.connect(update); From d464020577bbb3336e9153f9a21b55af65af03b8 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 10 Apr 2017 16:03:34 -0700 Subject: [PATCH 05/17] Adjust min angle of knee constraint to prevent leg locks --- 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 7fda318f5c..d6c976ce21 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -829,7 +829,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.0f; + const float MIN_KNEE_ANGLE = 0.097f; // ~5 deg const float MAX_KNEE_ANGLE = 7.0f * PI / 8.0f; glm::quat invReferenceRotation = glm::inverse(referenceRotation); glm::vec3 minSwingAxis = invReferenceRotation * glm::angleAxis(MIN_KNEE_ANGLE, hingeAxis) * Vectors::UNIT_Y; From b49760cee2a729cc177c7c3cc7a126489868ab98 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 10 Apr 2017 16:52:46 -0700 Subject: [PATCH 06/17] Remove hand IK target collision with body capsule --- libraries/animation/src/Rig.cpp | 20 ++------------------ 1 file changed, 2 insertions(+), 18 deletions(-) diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index c8717fd11e..dd7ccb6b27 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1174,15 +1174,7 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f const glm::vec3 bodyCapsuleEnd = bodyCapsuleCenter + glm::vec3(0, params.bodyCapsuleHalfHeight, 0); if (params.isLeftEnabled) { - - // prevent the hand IK targets from intersecting the body capsule - glm::vec3 handPosition = params.leftPosition; - glm::vec3 displacement; - if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) { - handPosition -= displacement; - } - - _animVars.set("leftHandPosition", handPosition); + _animVars.set("leftHandPosition", params.leftPosition); _animVars.set("leftHandRotation", params.leftOrientation); _animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition); } else { @@ -1192,15 +1184,7 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f } if (params.isRightEnabled) { - - // prevent the hand IK targets from intersecting the body capsule - glm::vec3 handPosition = params.rightPosition; - glm::vec3 displacement; - if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) { - handPosition -= displacement; - } - - _animVars.set("rightHandPosition", handPosition); + _animVars.set("rightHandPosition", params.rightPosition); _animVars.set("rightHandRotation", params.rightOrientation); _animVars.set("rightHandType", (int)IKTarget::Type::RotationAndPosition); } else { From 22e79504bb71a380e8f4692550e8aa25340e16ec Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 14 Apr 2017 16:57:30 -0700 Subject: [PATCH 07/17] Elliptical swing targets for the spine, Bug fix for debug draw --- .../animation/src/AnimInverseKinematics.cpp | 51 ++++++++++++------- 1 file changed, 34 insertions(+), 17 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index d6c976ce21..0e7a41e74c 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -149,7 +149,7 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector MAX_ERROR_TOLERANCE && numLoops < MAX_IK_LOOPS) { ++numLoops; @@ -365,7 +365,9 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe } // store the relative rotation change in the accumulator - _accumulators[pivotIndex].add(newRot, target.getWeight()); + // AJT: Hack give head more weight. + float weight = (target.getIndex() == _headIndex) ? 2.0f : 1.0f; + _accumulators[pivotIndex].add(newRot, weight); // this joint has been changed so we check to see if it has the lowest index if (pivotIndex < lowestMovedIndex) { @@ -445,8 +447,6 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars computeTargets(animVars, targets, underPoses); } - _previousEnableDebugIKTargets = context.getEnableDebugDrawIKTargets(); - if (targets.empty()) { // no IK targets but still need to enforce constraints std::map::iterator constraintItr = _constraints.begin(); @@ -510,6 +510,8 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars DebugDraw::getInstance().removeMyAvatarMarker(name); } } + + _previousEnableDebugIKTargets = context.getEnableDebugDrawIKTargets(); } { @@ -551,6 +553,22 @@ void AnimInverseKinematics::clearConstraints() { _constraints.clear(); } +// set up swing limits around a swingTwistConstraint in an ellipse, where lateralSwingTheta is the swing limit for lateral swings (side to side) +// anteriorSwingTheta is swing limit for forward and backward swings. (where x-axis of reference rotation is sideways and -z-axis is forward) +static void setEllipticalSwingLimits(SwingTwistConstraint* stConstraint, float lateralSwingTheta, float anteriorSwingTheta) { + assert(stConstraint); + const int NUM_SUBDIVISIONS = 8; + std::vector minDots; + minDots.reserve(NUM_SUBDIVISIONS); + float dTheta = (2.0f * PI) / NUM_SUBDIVISIONS; + float theta = 0.0f; + for (int i = 0; i < NUM_SUBDIVISIONS; i++) { + minDots.push_back(cosf(glm::length(glm::vec2(anteriorSwingTheta * cosf(theta), lateralSwingTheta * sinf(theta))))); + theta += dTheta; + } + stConstraint->setSwingLimits(minDots); +} + void AnimInverseKinematics::initConstraints() { if (!_skeleton) { return; @@ -740,41 +758,40 @@ void AnimInverseKinematics::initConstraints() { } else if (baseName.startsWith("Spine", Qt::CaseSensitive)) { SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot()); - const float MAX_SPINE_TWIST = PI / 12.0f; + const float MAX_SPINE_TWIST = PI / 20.0f; stConstraint->setTwistLimits(-MAX_SPINE_TWIST, MAX_SPINE_TWIST); + /* std::vector minDots; const float MAX_SPINE_SWING = PI / 10.0f; minDots.push_back(cosf(MAX_SPINE_SWING)); stConstraint->setSwingLimits(minDots); + */ + + // AJT: limit lateral swings more then forward-backward swings + setEllipticalSwingLimits(stConstraint, PI / 30.0f, PI / 20.0f); + if (0 == baseName.compare("Spine1", Qt::CaseSensitive) || 0 == baseName.compare("Spine", Qt::CaseSensitive)) { stConstraint->setLowerSpine(true); } constraint = static_cast(stConstraint); - } else if (baseName.startsWith("Hips2", Qt::CaseSensitive)) { - SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); - stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot()); - const float MAX_SPINE_TWIST = PI / 8.0f; - stConstraint->setTwistLimits(-MAX_SPINE_TWIST, MAX_SPINE_TWIST); - std::vector minDots; - const float MAX_SPINE_SWING = PI / 14.0f; - minDots.push_back(cosf(MAX_SPINE_SWING)); - stConstraint->setSwingLimits(minDots); - - constraint = static_cast(stConstraint); } else if (0 == baseName.compare("Neck", Qt::CaseSensitive)) { SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot()); - const float MAX_NECK_TWIST = PI / 9.0f; + const float MAX_NECK_TWIST = PI / 10.0f; stConstraint->setTwistLimits(-MAX_NECK_TWIST, MAX_NECK_TWIST); + /* std::vector minDots; const float MAX_NECK_SWING = PI / 8.0f; minDots.push_back(cosf(MAX_NECK_SWING)); stConstraint->setSwingLimits(minDots); + */ + + setEllipticalSwingLimits(stConstraint, PI / 10.0f, PI / 8.0f); constraint = static_cast(stConstraint); } else if (0 == baseName.compare("Head", Qt::CaseSensitive)) { From 47e51493e87cc3d36906bdf67e2d8079133c602e Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 24 Apr 2017 09:56:10 -0700 Subject: [PATCH 08/17] dynamicallyAdjustLimits on the underPoses not the relaxed poses. --- libraries/animation/src/AnimInverseKinematics.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 0e7a41e74c..a457adddd5 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -426,13 +426,13 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars _relativePoses[i].trans() = underPoses[i].trans(); } - if (!_relativePoses.empty()) { + if (!underPoses.empty()) { // Sometimes the underpose itself can violate the constraints. Rather than // clamp the animation we dynamically expand each constraint to accomodate it. std::map::iterator constraintItr = _constraints.begin(); while (constraintItr != _constraints.end()) { int index = constraintItr->first; - constraintItr->second->dynamicallyAdjustLimits(_relativePoses[index].rot()); + constraintItr->second->dynamicallyAdjustLimits(underPoses[index].rot()); ++constraintItr; } } From dc3803a2250d0c896ddce146729be50687af831e Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 24 Apr 2017 13:59:02 -0700 Subject: [PATCH 09/17] Re-enable IK _hipsOffset computation when no hips IK target is present. --- interface/src/avatar/SkeletonModel.cpp | 6 +- .../animation/src/AnimInverseKinematics.cpp | 140 ++++++++++++++---- .../animation/src/AnimInverseKinematics.h | 2 + libraries/animation/src/Rig.cpp | 32 +++- 4 files changed, 144 insertions(+), 36 deletions(-) diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index e26c339fb8..3cf866fb6b 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -120,8 +120,10 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) { headParams.rigHeadOrientation = extractRotation(rigHMDMat); headParams.worldHeadOrientation = extractRotation(worldHMDMat); - headParams.hipsMatrix = worldToRig * myAvatar->getSensorToWorldMatrix() * myAvatar->deriveBodyFromHMDSensor(); - headParams.hipsEnabled = true; + // TODO: if hips target sensor is valid. + // Copy it into headParams.hipsMatrix, and set headParams.hipsEnabled to true. + + headParams.hipsEnabled = false; } else { headParams.hipsEnabled = false; headParams.isInHMD = false; diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index a457adddd5..a9898fb4d2 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -86,6 +86,7 @@ void AnimInverseKinematics::setTargetVars( void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std::vector& targets, const AnimPoseVec& underPoses) { // build a list of valid targets from _targetVarVec and animVars _maxTargetIndex = -1; + _hipsTargetIndex = -1; bool removeUnfoundJoints = false; for (auto& targetVar : _targetVarVec) { @@ -114,6 +115,11 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: if (targetVar.jointIndex > _maxTargetIndex) { _maxTargetIndex = targetVar.jointIndex; } + + // record the index of the hips ik target. + if (target.getIndex() == _hipsIndex) { + _hipsTargetIndex = (int)targets.size() - 1; + } } } } @@ -242,6 +248,8 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe // the tip's parent-relative as we proceed up the chain glm::quat tipParentOrientation = absolutePoses[pivotIndex].rot(); + // AJT: REMOVE + /* if (targetType == IKTarget::Type::HmdHead) { // rotate tip directly to target orientation tipOrientation = target.getRotation(); @@ -259,6 +267,7 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe // store the relative rotation change in the accumulator _accumulators[tipIndex].add(tipRelativeRotation, target.getWeight()); } + */ // cache tip absolute position glm::vec3 tipPosition = absolutePoses[tipIndex].trans(); @@ -278,9 +287,8 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe const float MIN_AXIS_LENGTH = 1.0e-4f; RotationConstraint* constraint = getConstraint(pivotIndex); - // AJT: disabled special case for the lower spine. - /* - if (constraint && constraint->isLowerSpine() && tipIndex != _headIndex) { + // 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 // (this prevents the hand targets from bending the spine too much and thereby driving the hips too far) glm::vec3 twistAxis = absolutePoses[pivotIndex].trans() - absolutePoses[pivotsParentIndex].trans(); @@ -295,7 +303,6 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe targetLine = Vectors::ZERO; } } - */ glm::vec3 axis = glm::cross(leverArm, targetLine); float axisLength = glm::length(axis); @@ -365,9 +372,7 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe } // store the relative rotation change in the accumulator - // AJT: Hack give head more weight. - float weight = (target.getIndex() == _headIndex) ? 2.0f : 1.0f; - _accumulators[pivotIndex].add(newRot, weight); + _accumulators[pivotIndex].add(newRot, target.getWeight()); // this joint has been changed so we check to see if it has the lowest index if (pivotIndex < lowestMovedIndex) { @@ -448,41 +453,46 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars } if (targets.empty()) { - // no IK targets but still need to enforce constraints - std::map::iterator constraintItr = _constraints.begin(); - while (constraintItr != _constraints.end()) { - int index = constraintItr->first; - glm::quat rotation = _relativePoses[index].rot(); - constraintItr->second->apply(rotation); - _relativePoses[index].rot() = rotation; - ++constraintItr; - } + _relativePoses = underPoses; } else { { PROFILE_RANGE_EX(simulation_animation, "ik/shiftHips", 0xffff00ff, 0); - // AJT: TODO only need abs poses below hips. - AnimPoseVec absolutePoses; - absolutePoses.resize(_relativePoses.size()); - computeAbsolutePoses(absolutePoses); - - for (auto& target: targets) { - if (target.getType() == IKTarget::Type::RotationAndPosition && target.getIndex() == _hipsIndex) { - AnimPose absPose = target.getPose(); - int parentIndex = _skeleton->getParentIndex(target.getIndex()); - if (parentIndex != -1) { - _relativePoses[_hipsIndex] = absolutePoses[parentIndex].inverse() * absPose; + if (_hipsTargetIndex >= 0 && _hipsTargetIndex < targets.size()) { + // slam the hips to match the _hipsTarget + AnimPose absPose = targets[_hipsTargetIndex].getPose(); + int parentIndex = _skeleton->getParentIndex(targets[_hipsTargetIndex].getIndex()); + if (parentIndex != -1) { + _relativePoses[_hipsIndex] = _skeleton->getAbsolutePose(parentIndex, _relativePoses).inverse() * absPose; + } else { + _relativePoses[_hipsIndex] = absPose; + } + } else { + // if there is no hips target, shift hips according to the _hipsOffset from the previous frame + float offsetLength = glm::length(_hipsOffset); + const float MIN_HIPS_OFFSET_LENGTH = 0.03f; + if (offsetLength > MIN_HIPS_OFFSET_LENGTH && _hipsIndex >= 0) { + float scaleFactor = ((offsetLength - MIN_HIPS_OFFSET_LENGTH) / offsetLength); + glm::vec3 hipsOffset = scaleFactor * _hipsOffset; + if (_hipsParentIndex == -1) { + _relativePoses[_hipsIndex].trans() = underPoses[_hipsIndex].trans() + hipsOffset; } else { - _relativePoses[_hipsIndex] = absPose; + auto absHipsPose = _skeleton->getAbsolutePose(_hipsIndex, underPoses); + absHipsPose.trans() += hipsOffset; + _relativePoses[_hipsIndex] = _skeleton->getAbsolutePose(_hipsParentIndex, _relativePoses).inverse() * absHipsPose; } } } + // update all HipsRelative targets to account for the hips shift/ik target. + auto shiftedHipsAbsPose = _skeleton->getAbsolutePose(_hipsIndex, _relativePoses); + auto underHipsAbsPose = _skeleton->getAbsolutePose(_hipsIndex, underPoses); + auto absHipsOffset = shiftedHipsAbsPose.trans() - underHipsAbsPose.trans(); for (auto& target: targets) { if (target.getType() == IKTarget::Type::HipsRelativeRotationAndPosition) { - AnimPose pose = target.getPose(); - pose.trans() = pose.trans() + (_relativePoses[_hipsIndex].trans() - underPoses[_hipsIndex].trans()); + auto pose = target.getPose(); + pose.trans() = pose.trans() + absHipsOffset; target.setPose(pose.rot(), pose.trans()); } } @@ -518,11 +528,81 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars PROFILE_RANGE_EX(simulation_animation, "ik/ccd", 0xffff00ff, 0); solveWithCyclicCoordinateDescent(targets); } + + if (_hipsTargetIndex < 0) { + PROFILE_RANGE_EX(simulation_animation, "ik/measureHipsOffset", 0xffff00ff, 0); + computeHipsOffset(targets, underPoses, dt); + } else { + _hipsOffset = Vectors::ZERO; + } } } return _relativePoses; } +void AnimInverseKinematics::computeHipsOffset(const std::vector& targets, const AnimPoseVec& underPoses, float dt) { + // measure new _hipsOffset for next frame + // by looking for discrepancies between where a targeted endEffector is + // and where it wants to be (after IK solutions are done) + + // OUTOFBODY_HACK:use weighted average between HMD and other targets + float HMD_WEIGHT = 10.0f; + float OTHER_WEIGHT = 1.0f; + float totalWeight = 0.0f; + + glm::vec3 additionalHipsOffset = Vectors::ZERO; + for (auto& target: targets) { + int targetIndex = target.getIndex(); + if (targetIndex == _headIndex && _headIndex != -1) { + // special handling for headTarget + if (target.getType() == IKTarget::Type::RotationOnly) { + // we want to shift the hips to bring the underPose closer + // to where the head happens to be (overpose) + glm::vec3 under = _skeleton->getAbsolutePose(_headIndex, underPoses).trans(); + glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans(); + const float HEAD_OFFSET_SLAVE_FACTOR = 0.65f; + additionalHipsOffset += (OTHER_WEIGHT * HEAD_OFFSET_SLAVE_FACTOR) * (under - actual); + totalWeight += OTHER_WEIGHT; + } else if (target.getType() == IKTarget::Type::HmdHead) { + glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans(); + glm::vec3 thisOffset = target.getTranslation() - actual; + glm::vec3 futureHipsOffset = _hipsOffset + thisOffset; + if (glm::length(glm::vec2(futureHipsOffset.x, futureHipsOffset.z)) < _maxHipsOffsetLength) { + // it is imperative to shift the hips and bring the head to its designated position + // so we slam newHipsOffset here and ignore all other targets + additionalHipsOffset = futureHipsOffset - _hipsOffset; + totalWeight = 0.0f; + break; + } else { + additionalHipsOffset += HMD_WEIGHT * (target.getTranslation() - actual); + totalWeight += HMD_WEIGHT; + } + } + } else if (target.getType() == IKTarget::Type::RotationAndPosition) { + glm::vec3 actualPosition = _skeleton->getAbsolutePose(targetIndex, _relativePoses).trans(); + glm::vec3 targetPosition = target.getTranslation(); + additionalHipsOffset += OTHER_WEIGHT * (targetPosition - actualPosition); + totalWeight += OTHER_WEIGHT; + } + } + if (totalWeight > 1.0f) { + additionalHipsOffset /= totalWeight; + } + + // smooth transitions by relaxing _hipsOffset toward the new value + const float HIPS_OFFSET_SLAVE_TIMESCALE = 0.10f; + float tau = dt < HIPS_OFFSET_SLAVE_TIMESCALE ? dt / HIPS_OFFSET_SLAVE_TIMESCALE : 1.0f; + _hipsOffset += additionalHipsOffset * tau; + + // clamp the horizontal component of the hips offset + float hipsOffsetLength2D = glm::length(glm::vec2(_hipsOffset.x, _hipsOffset.z)); + if (hipsOffsetLength2D > _maxHipsOffsetLength) { + _hipsOffset.x *= _maxHipsOffsetLength / hipsOffsetLength2D; + _hipsOffset.z *= _maxHipsOffsetLength / hipsOffsetLength2D; + } + +} + void AnimInverseKinematics::setMaxHipsOffsetLength(float maxLength) { // manually adjust scale here const float METERS_TO_CENTIMETERS = 100.0f; diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index 366e5f765e..c91b7aa9c4 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -55,6 +55,7 @@ protected: RotationConstraint* getConstraint(int index); void clearConstraints(); void initConstraints(); + void computeHipsOffset(const std::vector& targets, const AnimPoseVec& underPoses, float dt); // no copies AnimInverseKinematics(const AnimInverseKinematics&) = delete; @@ -91,6 +92,7 @@ protected: int _headIndex { -1 }; int _hipsIndex { -1 }; int _hipsParentIndex { -1 }; + int _hipsTargetIndex { -1 }; // _maxTargetIndex is tracked to help optimize the recalculation of absolute poses // during the the cyclic coordinate descent algorithm diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index dd7ccb6b27..116758b1ba 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1025,7 +1025,6 @@ void Rig::updateFromHeadParameters(const HeadParameters& params, float dt) { _animVars.set("isTalking", params.isTalking); _animVars.set("notIsTalking", !params.isTalking); - // AJT: if (params.hipsEnabled) { _animVars.set("hipsType", (int)IKTarget::Type::RotationAndPosition); _animVars.set("hipsPosition", extractTranslation(params.hipsMatrix)); @@ -1106,7 +1105,7 @@ void Rig::updateNeckJoint(int index, const HeadParameters& params) { _animVars.set("headPosition", headPos); _animVars.set("headRotation", headRot); - _animVars.set("headType", (int)IKTarget::Type::RotationAndPosition); + _animVars.set("headType", (int)IKTarget::Type::HmdHead); _animVars.set("neckPosition", neckPos); _animVars.set("neckRotation", neckRot); _animVars.set("neckType", (int)IKTarget::Type::Unknown); // 'Unknown' disables the target @@ -1173,8 +1172,22 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f const glm::vec3 bodyCapsuleStart = bodyCapsuleCenter - glm::vec3(0, params.bodyCapsuleHalfHeight, 0); const glm::vec3 bodyCapsuleEnd = bodyCapsuleCenter + glm::vec3(0, params.bodyCapsuleHalfHeight, 0); + // TODO: add isHipsEnabled + bool bodySensorTrackingEnabled = params.isLeftFootEnabled || params.isRightFootEnabled; + if (params.isLeftEnabled) { - _animVars.set("leftHandPosition", params.leftPosition); + + glm::vec3 handPosition = params.leftPosition; + + 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("leftHandPosition", handPosition); _animVars.set("leftHandRotation", params.leftOrientation); _animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition); } else { @@ -1184,7 +1197,18 @@ void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, f } if (params.isRightEnabled) { - _animVars.set("rightHandPosition", params.rightPosition); + + glm::vec3 handPosition = params.rightPosition; + + 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", params.rightOrientation); _animVars.set("rightHandType", (int)IKTarget::Type::RotationAndPosition); } else { From 1cd0f032421b6fdef13e2a11a12fc6d7a9d63b54 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 24 Apr 2017 14:19:06 -0700 Subject: [PATCH 10/17] Restore master version of computeHipsOffset() and special case for HeadHMD target type --- .../animation/src/AnimInverseKinematics.cpp | 60 +++++++------------ 1 file changed, 22 insertions(+), 38 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index a9898fb4d2..ce199704a0 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -107,7 +107,6 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: 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(glm::vec3(1.0f), rotation, translation); target.setPose(rotation, translation); target.setIndex(targetVar.jointIndex); @@ -155,7 +154,7 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector MAX_ERROR_TOLERANCE && numLoops < MAX_IK_LOOPS) { ++numLoops; @@ -248,8 +247,6 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe // the tip's parent-relative as we proceed up the chain glm::quat tipParentOrientation = absolutePoses[pivotIndex].rot(); - // AJT: REMOVE - /* if (targetType == IKTarget::Type::HmdHead) { // rotate tip directly to target orientation tipOrientation = target.getRotation(); @@ -267,7 +264,6 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe // store the relative rotation change in the accumulator _accumulators[tipIndex].add(tipRelativeRotation, target.getWeight()); } - */ // cache tip absolute position glm::vec3 tipPosition = absolutePoses[tipIndex].trans(); @@ -544,13 +540,7 @@ void AnimInverseKinematics::computeHipsOffset(const std::vector& targe // measure new _hipsOffset for next frame // by looking for discrepancies between where a targeted endEffector is // and where it wants to be (after IK solutions are done) - - // OUTOFBODY_HACK:use weighted average between HMD and other targets - float HMD_WEIGHT = 10.0f; - float OTHER_WEIGHT = 1.0f; - float totalWeight = 0.0f; - - glm::vec3 additionalHipsOffset = Vectors::ZERO; + glm::vec3 newHipsOffset = Vectors::ZERO; for (auto& target: targets) { int targetIndex = target.getIndex(); if (targetIndex == _headIndex && _headIndex != -1) { @@ -561,46 +551,40 @@ void AnimInverseKinematics::computeHipsOffset(const std::vector& targe glm::vec3 under = _skeleton->getAbsolutePose(_headIndex, underPoses).trans(); glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans(); const float HEAD_OFFSET_SLAVE_FACTOR = 0.65f; - additionalHipsOffset += (OTHER_WEIGHT * HEAD_OFFSET_SLAVE_FACTOR) * (under - actual); - totalWeight += OTHER_WEIGHT; + newHipsOffset += HEAD_OFFSET_SLAVE_FACTOR * (actual - under); } else if (target.getType() == IKTarget::Type::HmdHead) { + // we want to shift the hips to bring the head to its designated position glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans(); - glm::vec3 thisOffset = target.getTranslation() - actual; - glm::vec3 futureHipsOffset = _hipsOffset + thisOffset; - if (glm::length(glm::vec2(futureHipsOffset.x, futureHipsOffset.z)) < _maxHipsOffsetLength) { - // it is imperative to shift the hips and bring the head to its designated position - // so we slam newHipsOffset here and ignore all other targets - additionalHipsOffset = futureHipsOffset - _hipsOffset; - totalWeight = 0.0f; - break; - } else { - additionalHipsOffset += HMD_WEIGHT * (target.getTranslation() - actual); - totalWeight += HMD_WEIGHT; - } + _hipsOffset += target.getTranslation() - actual; + // and ignore all other targets + newHipsOffset = _hipsOffset; + break; + } else if (target.getType() == IKTarget::Type::RotationAndPosition) { + glm::vec3 actualPosition = _skeleton->getAbsolutePose(targetIndex, _relativePoses).trans(); + glm::vec3 targetPosition = target.getTranslation(); + newHipsOffset += targetPosition - actualPosition; + + // Add downward pressure on the hips + newHipsOffset *= 0.95f; + newHipsOffset -= 1.0f; } } else if (target.getType() == IKTarget::Type::RotationAndPosition) { glm::vec3 actualPosition = _skeleton->getAbsolutePose(targetIndex, _relativePoses).trans(); glm::vec3 targetPosition = target.getTranslation(); - additionalHipsOffset += OTHER_WEIGHT * (targetPosition - actualPosition); - totalWeight += OTHER_WEIGHT; + newHipsOffset += targetPosition - actualPosition; } } - if (totalWeight > 1.0f) { - additionalHipsOffset /= totalWeight; - } // smooth transitions by relaxing _hipsOffset toward the new value const float HIPS_OFFSET_SLAVE_TIMESCALE = 0.10f; float tau = dt < HIPS_OFFSET_SLAVE_TIMESCALE ? dt / HIPS_OFFSET_SLAVE_TIMESCALE : 1.0f; - _hipsOffset += additionalHipsOffset * tau; + _hipsOffset += (newHipsOffset - _hipsOffset) * tau; - // clamp the horizontal component of the hips offset - float hipsOffsetLength2D = glm::length(glm::vec2(_hipsOffset.x, _hipsOffset.z)); - if (hipsOffsetLength2D > _maxHipsOffsetLength) { - _hipsOffset.x *= _maxHipsOffsetLength / hipsOffsetLength2D; - _hipsOffset.z *= _maxHipsOffsetLength / hipsOffsetLength2D; + // clamp the hips offset + float hipsOffsetLength = glm::length(_hipsOffset); + if (hipsOffsetLength > _maxHipsOffsetLength) { + _hipsOffset *= _maxHipsOffsetLength / hipsOffsetLength; } - } void AnimInverseKinematics::setMaxHipsOffsetLength(float maxLength) { From deca26e9eba3d405458742a2394bc9a7a1d5cbf0 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 24 Apr 2017 14:33:14 -0700 Subject: [PATCH 11/17] Fix for HMDHead tip constraint --- 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 ce199704a0..9f6a9ed56f 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -248,17 +248,18 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe glm::quat tipParentOrientation = absolutePoses[pivotIndex].rot(); if (targetType == IKTarget::Type::HmdHead) { + // rotate tip directly to target orientation tipOrientation = target.getRotation(); - glm::quat tipRelativeRotation = glm::normalize(tipOrientation * glm::inverse(tipParentOrientation)); + glm::quat tipRelativeRotation = glm::inverse(tipParentOrientation) * tipOrientation; - // enforce tip's constraint + // then enforce tip's constraint RotationConstraint* constraint = getConstraint(tipIndex); if (constraint) { bool constrained = constraint->apply(tipRelativeRotation); if (constrained) { - tipOrientation = glm::normalize(tipRelativeRotation * tipParentOrientation); - tipRelativeRotation = glm::normalize(tipOrientation * glm::inverse(tipParentOrientation)); + tipOrientation = tipParentOrientation * tipRelativeRotation; + tipRelativeRotation = tipRelativeRotation; } } // store the relative rotation change in the accumulator From 8adbe34c2736e20e2dc77c335af27b5f3fdf97a7 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 24 Apr 2017 14:36:56 -0700 Subject: [PATCH 12/17] added comment --- libraries/animation/src/AnimInverseKinematics.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 9f6a9ed56f..05a5d3bac6 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -247,6 +247,8 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe // the tip's parent-relative as we proceed up the chain glm::quat tipParentOrientation = absolutePoses[pivotIndex].rot(); + // NOTE: if this code is removed, the head will remain rigid, causing the spine/hips to thrust forward backward + // as the head is nodded. if (targetType == IKTarget::Type::HmdHead) { // rotate tip directly to target orientation From 8b16a7c7e19ba4496a3136db154286244b8ddee6 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 24 Apr 2017 16:40:24 -0700 Subject: [PATCH 13/17] Added hipsIkTest.js test script --- scripts/developer/tests/hipsIkTest.js | 118 ++++++++++++++++++++++++++ 1 file changed, 118 insertions(+) create mode 100644 scripts/developer/tests/hipsIkTest.js diff --git a/scripts/developer/tests/hipsIkTest.js b/scripts/developer/tests/hipsIkTest.js new file mode 100644 index 0000000000..340d1ae7a0 --- /dev/null +++ b/scripts/developer/tests/hipsIkTest.js @@ -0,0 +1,118 @@ +// +// hipsIKTest.js +// +// Created by Anthony Thibault on 4/24/17 +// Copyright 2017 High Fidelity, Inc. +// +// Test procedural manipulation of the Avatar hips IK target. +// Pull the left and right triggers on your hand controllers, you hips should begin to gyrate in an amusing mannor. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html + +/* global Xform */ +Script.include("/~/system/libraries/Xform.js"); + +var calibrated = false; +var rightTriggerPressed = false; +var leftTriggerPressed = false; + +var MAPPING_NAME = "com.highfidelity.hipsIkTest"; + +var mapping = Controller.newMapping(MAPPING_NAME); +mapping.from([Controller.Standard.RTClick]).peek().to(function (value) { + rightTriggerPressed = (value !== 0) ? true : false; +}); +mapping.from([Controller.Standard.LTClick]).peek().to(function (value) { + leftTriggerPressed = (value !== 0) ? true : false; +}); + +Controller.enableMapping(MAPPING_NAME); + +var ANIM_VARS = [ + "headType", + "hipsType", + "hipsPosition", + "hipsRotation" +]; + +var ZERO = {x: 0, y: 0, z: 0}; +var X_AXIS = {x: 1, y: 0, z: 0}; +var Y_AXIS = {x: 0, y: 1, z: 0}; +var Y_180 = {x: 0, y: 1, z: 0, w: 0}; +var Y_180_XFORM = new Xform(Y_180, {x: 0, y: 0, z: 0}); + +var hips = undefined; + +function computeCurrentXform(jointIndex) { + var currentXform = new Xform(MyAvatar.getAbsoluteJointRotationInObjectFrame(jointIndex), + MyAvatar.getAbsoluteJointTranslationInObjectFrame(jointIndex)); + return Xform.mul(Y_180_XFORM, currentXform); +} + +function calibrate() { + hips = computeCurrentXform(MyAvatar.getJointIndex("Hips")); +} + +var ikTypes = { + RotationAndPosition: 0, + RotationOnly: 1, + HmdHead: 2, + HipsRelativeRotationAndPosition: 3, + Off: 4 +}; + +function circleOffset(radius, theta, normal) { + var pos = {x: radius * Math.cos(theta), y: radius * Math.sin(theta), z: 0}; + var lookAtRot = Quat.lookAt(normal, ZERO, X_AXIS); + return Vec3.multiplyQbyV(lookAtRot, pos); +} + +var handlerId; + +function update(dt) { + if (rightTriggerPressed && leftTriggerPressed) { + if (!calibrated) { + calibrate(); + calibrated = true; + + if (handlerId) { + MyAvatar.removeAnimationStateHandler(handlerId); + handlerId = undefined; + } else { + + var n = Y_AXIS; + var t = 0; + handlerId = MyAvatar.addAnimationStateHandler(function (props) { + + t += (1 / 60) * 4; + var result = {}, xform; + if (hips) { + xform = hips; + result.hipsType = ikTypes.RotationAndPosition; + result.hipsPosition = Vec3.sum(xform.pos, circleOffset(0.1, t, n)); + result.hipsRotation = xform.rot; + result.headType = ikTypes.RotationAndPosition; + } else { + result.headType = ikTypes.HmdHead; + result.hipsType = props.hipsType; + result.hipsPosition = props.hipsPosition; + result.hipsRotation = props.hipsRotation; + } + + return result; + }, ANIM_VARS); + } + } + } else { + calibrated = false; + } +} + +Script.update.connect(update); + +Script.scriptEnding.connect(function () { + Controller.disableMapping(MAPPING_NAME); + Script.update.disconnect(update); +}); + From 937f308ba86bcc63e07f0bf30a62089553cd4a6a Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 24 Apr 2017 17:04:16 -0700 Subject: [PATCH 14/17] code cleanup --- interface/src/avatar/MyAvatar.h | 9 ++++----- .../animation/src/AnimInverseKinematics.cpp | 16 +--------------- 2 files changed, 5 insertions(+), 20 deletions(-) diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index b5bea23aa3..0146bd11a4 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -465,6 +465,10 @@ public: void removeHoldAction(AvatarActionHold* holdAction); // thread-safe void updateHoldActions(const AnimPose& prePhysicsPose, const AnimPose& postUpdatePose); + // derive avatar body position and orientation from the current HMD Sensor location. + // results are in HMD frame + glm::mat4 deriveBodyFromHMDSensor() const; + public slots: void increaseSize(); void decreaseSize(); @@ -553,11 +557,6 @@ private: void setVisibleInSceneIfReady(Model* model, render::ScenePointer scene, bool visiblity); - // AJT: FIX ME... reorder this -public: - // derive avatar body position and orientation from the current HMD Sensor location. - // results are in HMD frame - glm::mat4 deriveBodyFromHMDSensor() const; private: virtual void updatePalms() override {} diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 05a5d3bac6..83419afe65 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -828,14 +828,7 @@ void AnimInverseKinematics::initConstraints() { const float MAX_SPINE_TWIST = PI / 20.0f; stConstraint->setTwistLimits(-MAX_SPINE_TWIST, MAX_SPINE_TWIST); - /* - std::vector minDots; - const float MAX_SPINE_SWING = PI / 10.0f; - minDots.push_back(cosf(MAX_SPINE_SWING)); - stConstraint->setSwingLimits(minDots); - */ - - // AJT: limit lateral swings more then forward-backward swings + // limit lateral swings more then forward-backward swings setEllipticalSwingLimits(stConstraint, PI / 30.0f, PI / 20.0f); if (0 == baseName.compare("Spine1", Qt::CaseSensitive) @@ -851,13 +844,6 @@ void AnimInverseKinematics::initConstraints() { const float MAX_NECK_TWIST = PI / 10.0f; stConstraint->setTwistLimits(-MAX_NECK_TWIST, MAX_NECK_TWIST); - /* - std::vector minDots; - const float MAX_NECK_SWING = PI / 8.0f; - minDots.push_back(cosf(MAX_NECK_SWING)); - stConstraint->setSwingLimits(minDots); - */ - setEllipticalSwingLimits(stConstraint, PI / 10.0f, PI / 8.0f); constraint = static_cast(stConstraint); From 4336e22f5a56752c2c2d954cb280f2abf6ee76ae Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Mon, 24 Apr 2017 17:06:02 -0700 Subject: [PATCH 15/17] clang warning fix --- 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 83419afe65..b36abebb1b 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -458,7 +458,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars { PROFILE_RANGE_EX(simulation_animation, "ik/shiftHips", 0xffff00ff, 0); - if (_hipsTargetIndex >= 0 && _hipsTargetIndex < targets.size()) { + if (_hipsTargetIndex >= 0 && _hipsTargetIndex < (int)targets.size()) { // slam the hips to match the _hipsTarget AnimPose absPose = targets[_hipsTargetIndex].getPose(); int parentIndex = _skeleton->getParentIndex(targets[_hipsTargetIndex].getIndex()); From ce8b71ff9464669ad324ee814ee8c93f447da02b Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 25 Apr 2017 15:05:52 -0700 Subject: [PATCH 16/17] viveMotionCapture.js: can now disable puck control, with a second controller squeeze --- scripts/developer/tests/viveMotionCapture.js | 130 +++++++++---------- 1 file changed, 65 insertions(+), 65 deletions(-) diff --git a/scripts/developer/tests/viveMotionCapture.js b/scripts/developer/tests/viveMotionCapture.js index 6cb0f92b9b..5496b475be 100644 --- a/scripts/developer/tests/viveMotionCapture.js +++ b/scripts/developer/tests/viveMotionCapture.js @@ -8,7 +8,7 @@ var TRACKED_OBJECT_POSES = [ "TrackedObject12", "TrackedObject13", "TrackedObject14", "TrackedObject15" ]; -var calibrated = false; +var triggerPressHandled = false; var rightTriggerPressed = false; var leftTriggerPressed = false; @@ -43,21 +43,6 @@ var SENSOR_CONFIG_NAMES = [ "Auto" ]; -var ANIM_VARS = [ - "leftFootType", - "leftFootPosition", - "leftFootRotation", - "rightFootType", - "rightFootPosition", - "rightFootRotation", - "hipsType", - "hipsPosition", - "hipsRotation", - "spine2Type", - "spine2Position", - "spine2Rotation" -]; - var sensorConfig = AUTO; var Y_180 = {x: 0, y: 1, z: 0, w: 0}; @@ -86,7 +71,7 @@ function computeDefaultToReferenceXform() { return defaultToReferenceXform; } else { - return new Xform.ident(); + return Xform.ident(); } } @@ -200,71 +185,86 @@ function computeIKTargetXform(jointInfo) { function update(dt) { if (rightTriggerPressed && leftTriggerPressed) { - if (!calibrated) { - calibrate(); - calibrated = true; - + if (!triggerPressHandled) { + triggerPressHandled = true; if (handlerId) { - MyAvatar.removeAnimationStateHandler(handlerId); - } + print("AJT: UN-CALIBRATE!"); - handlerId = MyAvatar.addAnimationStateHandler(function (props) { - - var result = {}, xform; - if (rightFoot) { - xform = computeIKTargetXform(rightFoot); - result.rightFootType = ikTypes.RotationAndPosition; - result.rightFootPosition = xform.pos; - result.rightFootRotation = xform.rot; - } else { - result.rightFootType = props.rightFootType; - result.rightFootPosition = props.rightFootPosition; - result.rightFootRotation = props.rightFootRotation; + // go back to normal, vive pucks will be ignored. + leftFoot = undefined; + rightFoot = undefined; + hips = undefined; + spine2 = undefined; + if (handlerId) { + print("AJT: un-hooking animation state handler"); + MyAvatar.removeAnimationStateHandler(handlerId); + handlerId = undefined; } + } else { + print("AJT: CALIBRATE!"); + calibrate(); + + var animVars = []; if (leftFoot) { - xform = computeIKTargetXform(leftFoot); - result.leftFootType = ikTypes.RotationAndPosition; - result.leftFootPosition = xform.pos; - result.leftFootRotation = xform.rot; - } else { - result.leftFootType = props.leftFootType; - result.leftFootPosition = props.leftFootPosition; - result.leftFootRotation = props.leftFootRotation; + animVars.push("leftFootType"); + animVars.push("leftFootPosition"); + animVars.push("leftFootRotation"); + } + if (rightFoot) { + animVars.push("rightFootType"); + animVars.push("rightFootPosition"); + animVars.push("rightFootRotation"); } - if (hips) { - xform = computeIKTargetXform(hips); - result.hipsType = ikTypes.RotationAndPosition; - result.hipsPosition = xform.pos; - result.hipsRotation = xform.rot; - } else { - result.hipsType = props.hipsType; - result.hipsPosition = props.hipsPosition; - result.hipsRotation = props.hipsRotation; + animVars.push("hipsType"); + animVars.push("hipsPosition"); + animVars.push("hipsRotation"); } - if (spine2) { - xform = computeIKTargetXform(spine2); - result.spine2Type = ikTypes.RotationAndPosition; - result.spine2Position = xform.pos; - result.spine2Rotation = xform.rot; - } else { - result.spine2Type = ikTypes.Off; + animVars.push("spine2Type"); + animVars.push("spine2Position"); + animVars.push("spine2Rotation"); } - return result; - }, ANIM_VARS); - + // hook up new anim state handler that maps vive pucks to ik system. + handlerId = MyAvatar.addAnimationStateHandler(function (props) { + var result = {}, xform; + if (rightFoot) { + xform = computeIKTargetXform(rightFoot); + result.rightFootType = ikTypes.RotationAndPosition; + result.rightFootPosition = xform.pos; + result.rightFootRotation = xform.rot; + } + if (leftFoot) { + xform = computeIKTargetXform(leftFoot); + result.leftFootType = ikTypes.RotationAndPosition; + result.leftFootPosition = xform.pos; + result.leftFootRotation = xform.rot; + } + if (hips) { + xform = computeIKTargetXform(hips); + result.hipsType = ikTypes.RotationAndPosition; + result.hipsPosition = xform.pos; + result.hipsRotation = xform.rot; + } + if (spine2) { + xform = computeIKTargetXform(spine2); + result.spine2Type = ikTypes.RotationAndPosition; + result.spine2Position = xform.pos; + result.spine2Rotation = xform.rot; + } + return result; + }, animVars); + } } } else { - calibrated = false; + triggerPressHandled = false; } var drawMarkers = false; if (drawMarkers) { var RED = {x: 1, y: 0, z: 0, w: 1}; - var GREEN = {x: 0, y: 1, z: 0, w: 1}; var BLUE = {x: 0, y: 0, z: 1, w: 1}; if (leftFoot) { @@ -304,4 +304,4 @@ Script.scriptEnding.connect(function () { Controller.disableMapping(MAPPING_NAME); Script.update.disconnect(update); }); -var TRIGGER_OFF_VALUE = 0.1; + From c61e6a8a9e0e392a9455d96f082d7b4923292273 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 27 Apr 2017 18:24:21 -0700 Subject: [PATCH 17/17] AnimInverseKinematics: reduce number of magic constants --- .../animation/src/AnimInverseKinematics.cpp | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index b36abebb1b..6edd969568 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -568,8 +568,10 @@ void AnimInverseKinematics::computeHipsOffset(const std::vector& targe newHipsOffset += targetPosition - actualPosition; // Add downward pressure on the hips - newHipsOffset *= 0.95f; - newHipsOffset -= 1.0f; + const float PRESSURE_SCALE_FACTOR = 0.95f; + const float PRESSURE_TRANSLATION_OFFSET = 1.0f; + newHipsOffset *= PRESSURE_SCALE_FACTOR; + newHipsOffset -= PRESSURE_TRANSLATION_OFFSET; } } else if (target.getType() == IKTarget::Type::RotationAndPosition) { glm::vec3 actualPosition = _skeleton->getAbsolutePose(targetIndex, _relativePoses).trans(); @@ -627,7 +629,7 @@ static void setEllipticalSwingLimits(SwingTwistConstraint* stConstraint, float l const int NUM_SUBDIVISIONS = 8; std::vector minDots; minDots.reserve(NUM_SUBDIVISIONS); - float dTheta = (2.0f * PI) / NUM_SUBDIVISIONS; + float dTheta = TWO_PI / NUM_SUBDIVISIONS; float theta = 0.0f; for (int i = 0; i < NUM_SUBDIVISIONS; i++) { minDots.push_back(cosf(glm::length(glm::vec2(anteriorSwingTheta * cosf(theta), lateralSwingTheta * sinf(theta))))); @@ -829,7 +831,9 @@ void AnimInverseKinematics::initConstraints() { stConstraint->setTwistLimits(-MAX_SPINE_TWIST, MAX_SPINE_TWIST); // limit lateral swings more then forward-backward swings - setEllipticalSwingLimits(stConstraint, PI / 30.0f, PI / 20.0f); + const float MAX_SPINE_LATERAL_SWING = PI / 30.0f; + const float MAX_SPINE_ANTERIOR_SWING = PI / 20.0f; + setEllipticalSwingLimits(stConstraint, MAX_SPINE_LATERAL_SWING, MAX_SPINE_ANTERIOR_SWING); if (0 == baseName.compare("Spine1", Qt::CaseSensitive) || 0 == baseName.compare("Spine", Qt::CaseSensitive)) { @@ -844,7 +848,10 @@ void AnimInverseKinematics::initConstraints() { const float MAX_NECK_TWIST = PI / 10.0f; stConstraint->setTwistLimits(-MAX_NECK_TWIST, MAX_NECK_TWIST); - setEllipticalSwingLimits(stConstraint, PI / 10.0f, PI / 8.0f); + // limit lateral swings more then forward-backward swings + const float MAX_NECK_LATERAL_SWING = PI / 10.0f; + const float MAX_NECK_ANTERIOR_SWING = PI / 8.0f; + setEllipticalSwingLimits(stConstraint, MAX_NECK_LATERAL_SWING, MAX_NECK_ANTERIOR_SWING); constraint = static_cast(stConstraint); } else if (0 == baseName.compare("Head", Qt::CaseSensitive)) {