diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index ea87fab4c0..5ea628d1f4 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -150,24 +150,26 @@ void AnimInverseKinematics::setTargetVars(const QString& jointName, const QStrin } 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; + + targets.reserve(_targetVarVec.size()); for (auto& targetVar : _targetVarVec) { + + // update targetVar jointIndex cache if (targetVar.jointIndex == -1) { - // this targetVar hasn't been validated yet... int jointIndex = _skeleton->nameToJointIndex(targetVar.jointName); if (jointIndex >= 0) { // this targetVar has a valid joint --> cache the indices targetVar.jointIndex = jointIndex; } else { qCWarning(animation) << "AnimInverseKinematics could not find jointName" << targetVar.jointName << "in skeleton"; - removeUnfoundJoints = true; } - } else { - IKTarget target; + } + + IKTarget target; + if (targetVar.jointIndex != -1) { target.setType(animVars.lookup(targetVar.typeVar, (int)IKTarget::Type::RotationAndPosition)); if (target.getType() != IKTarget::Type::Unknown) { AnimPose absPose = _skeleton->getAbsolutePose(targetVar.jointIndex, underPoses); @@ -189,35 +191,16 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: glm::vec3 poleReferenceVector = animVars.lookupRigToGeometryVector(targetVar.poleReferenceVectorVar, Vectors::UNIT_Z); target.setPoleReferenceVector(glm::normalize(poleReferenceVector)); - targets.push_back(target); - - if (targetVar.jointIndex > _maxTargetIndex) { - _maxTargetIndex = targetVar.jointIndex; - } - // record the index of the hips ik target. if (target.getIndex() == _hipsIndex) { - _hipsTargetIndex = (int)targets.size() - 1; + _hipsTargetIndex = (int)targets.size(); } } + } else { + target.setType((int)IKTarget::Type::Unknown); } - } - if (removeUnfoundJoints) { - int numVars = (int)_targetVarVec.size(); - int i = 0; - while (i < numVars) { - if (_targetVarVec[i].jointIndex == -1) { - if (numVars > 1) { - // swap i for last element - _targetVarVec[i] = _targetVarVec[numVars - 1]; - } - _targetVarVec.pop_back(); - --numVars; - } else { - ++i; - } - } + targets.push_back(target); } } @@ -258,10 +241,15 @@ void AnimInverseKinematics::solve(const AnimContext& context, const std::vector< // solve all targets for (size_t i = 0; i < targets.size(); i++) { - if (targets[i].getType() == IKTarget::Type::Spline) { + switch (targets[i].getType()) { + case IKTarget::Type::Unknown: + break; + case IKTarget::Type::Spline: solveTargetWithSpline(context, targets[i], absolutePoses, debug, jointChainInfoVec[i]); - } else { + break; + default: solveTargetWithCCD(context, targets[i], absolutePoses, debug, jointChainInfoVec[i]); + break; } } @@ -317,7 +305,7 @@ void AnimInverseKinematics::solve(const AnimContext& context, const std::vector< // finally set the relative rotation of each tip to agree with absolute target rotation for (auto& target: targets) { int tipIndex = target.getIndex(); - int parentIndex = _skeleton->getParentIndex(tipIndex); + int parentIndex = (tipIndex >= 0) ? _skeleton->getParentIndex(tipIndex) : -1; // update rotationOnly targets that don't lie on the ik chain of other ik targets. if (parentIndex != -1 && !_rotationAccumulators[tipIndex].isDirty() && target.getType() == IKTarget::Type::RotationOnly) { @@ -1430,8 +1418,6 @@ void AnimInverseKinematics::setSkeletonInternal(AnimSkeleton::ConstPointer skele targetVar.jointIndex = -1; } - _maxTargetIndex = -1; - for (auto& accumulator: _rotationAccumulators) { accumulator.clearAndClean(); } diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index 38288aa288..fb462cbf50 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -156,10 +156,6 @@ protected: int _leftHandIndex { -1 }; int _rightHandIndex { -1 }; - // _maxTargetIndex is tracked to help optimize the recalculation of absolute poses - // during the the cyclic coordinate descent algorithm - int _maxTargetIndex { 0 }; - float _maxErrorOnLastSolve { FLT_MAX }; bool _previousEnableDebugIKTargets { false }; SolutionSource _solutionSource { SolutionSource::RelaxToUnderPoses }; diff --git a/libraries/animation/src/IKTarget.h b/libraries/animation/src/IKTarget.h index 5567539659..a86ae0ca8b 100644 --- a/libraries/animation/src/IKTarget.h +++ b/libraries/animation/src/IKTarget.h @@ -57,7 +57,7 @@ private: bool _poleVectorEnabled { false }; int _index { -1 }; Type _type { Type::RotationAndPosition }; - float _weight; + float _weight { 0.0f }; float _flexCoefficients[MAX_FLEX_COEFFICIENTS]; size_t _numFlexCoefficients; }; diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 3d04b0b26f..7b11465062 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1707,36 +1707,38 @@ void Rig::computeAvatarBoundingCapsule( AnimPose geometryToRig = _modelOffset * _geometryOffset; - AnimPose hips(glm::vec3(1), glm::quat(), glm::vec3()); + glm::vec3 hipsPosition(0.0f); int hipsIndex = indexOfJoint("Hips"); if (hipsIndex >= 0) { - hips = geometryToRig * _animSkeleton->getAbsoluteBindPose(hipsIndex); + hipsPosition = transformPoint(_geometryToRigTransform, _animSkeleton->getAbsoluteDefaultPose(hipsIndex).trans()); } AnimVariantMap animVars; + animVars.setRigToGeometryTransform(_rigToGeometryTransform); glm::quat handRotation = glm::angleAxis(PI, Vectors::UNIT_X); - animVars.set("leftHandPosition", hips.trans()); + animVars.set("leftHandPosition", hipsPosition); animVars.set("leftHandRotation", handRotation); animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition); - animVars.set("rightHandPosition", hips.trans()); + animVars.set("rightHandPosition", hipsPosition); animVars.set("rightHandRotation", handRotation); animVars.set("rightHandType", (int)IKTarget::Type::RotationAndPosition); int rightFootIndex = indexOfJoint("RightFoot"); int leftFootIndex = indexOfJoint("LeftFoot"); if (rightFootIndex != -1 && leftFootIndex != -1) { - glm::vec3 foot = Vectors::ZERO; + glm::vec3 geomFootPosition = glm::vec3(0.0f, _animSkeleton->getAbsoluteDefaultPose(rightFootIndex).trans().y, 0.0f); + glm::vec3 footPosition = transformPoint(_geometryToRigTransform, geomFootPosition); glm::quat footRotation = glm::angleAxis(0.5f * PI, Vectors::UNIT_X); - animVars.set("leftFootPosition", foot); + animVars.set("leftFootPosition", footPosition); animVars.set("leftFootRotation", footRotation); animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition); - animVars.set("rightFootPosition", foot); + animVars.set("rightFootPosition", footPosition); animVars.set("rightFootRotation", footRotation); animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition); } // call overlay twice: once to verify AnimPoseVec joints and again to do the IK AnimNode::Triggers triggersOut; - AnimContext context(false, false, false, glm::mat4(), glm::mat4()); + AnimContext context(false, false, false, _geometryToRigTransform, _rigToGeometryTransform); float dt = 1.0f; // the value of this does not matter ikNode.overlay(animVars, context, dt, triggersOut, _animSkeleton->getRelativeBindPoses()); AnimPoseVec finalPoses = ikNode.overlay(animVars, context, dt, triggersOut, _animSkeleton->getRelativeBindPoses());