diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index 376937fb13..4241faa616 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -344,114 +344,10 @@ void SkeletonModel::computeBoundingShape() { return; } - /* - AJT: TODO HACK DISABLED FIXME - - // BOUNDING SHAPE HACK: before we measure the bounds of the joints we use IK to put the - // hands and feet into positions that are more correct than the default pose. - - // Measure limb lengths so we can specify IK targets that will pull hands and feet tight to body - QVector endEffectors; - endEffectors.push_back("RightHand"); - endEffectors.push_back("LeftHand"); - endEffectors.push_back("RightFoot"); - endEffectors.push_back("LeftFoot"); - - QVector baseJoints; - baseJoints.push_back("RightArm"); - baseJoints.push_back("LeftArm"); - baseJoints.push_back("RightUpLeg"); - baseJoints.push_back("LeftUpLeg"); - - for (int i = 0; i < endEffectors.size(); ++i) { - QString tipName = endEffectors[i]; - QString baseName = baseJoints[i]; - float limbLength = 0.0f; - int tipIndex = _rig->indexOfJoint(tipName); - if (tipIndex == -1) { - continue; - } - // save tip's relative rotation for later - glm::quat tipRotation = _rig->getJointState(tipIndex).getRotationInConstrainedFrame(); - - // IK on each endpoint - int jointIndex = tipIndex; - QVector freeLineage; - float priority = 1.0f; - while (jointIndex > -1) { - JointState limbJoint = _rig->getJointState(jointIndex); - freeLineage.push_back(jointIndex); - if (limbJoint.getName() == baseName) { - glm::vec3 targetPosition = limbJoint.getPosition() - glm::vec3(0.0f, 1.5f * limbLength, 0.0f); - // do IK a few times to make sure the endpoint gets close to its target - for (int j = 0; j < 5; ++j) { - _rig->inverseKinematics(tipIndex, - targetPosition, - glm::quat(), - priority, - freeLineage, - glm::mat4()); - } - break; - } - limbLength += limbJoint.getDistanceToParent(); - jointIndex = limbJoint.getParentIndex(); - } - - // since this IK target is totally bogus we restore the tip's relative rotation - _rig->setJointRotationInConstrainedFrame(tipIndex, tipRotation, priority); - } - - // recompute all joint model-frame transforms - glm::mat4 rootTransform = glm::scale(_scale) * glm::translate(_offset) * geometry.offset; - for (int i = 0; i < _rig->getJointStateCount(); i++) { - _rig->updateJointState(i, rootTransform); - } - // END BOUNDING SHAPE HACK - - // compute bounding box that encloses all shapes - Extents totalExtents; - totalExtents.reset(); - totalExtents.addPoint(glm::vec3(0.0f)); - int numStates = _rig->getJointStateCount(); - for (int i = 0; i < numStates; i++) { - // Each joint contributes a capsule defined by FBXJoint.shapeInfo. - // For totalExtents we use the capsule endpoints expanded by the radius. - const JointState& state = _rig->getJointState(i); - const glm::mat4& jointTransform = state.getTransform(); - const FBXJointShapeInfo& shapeInfo = geometry.joints.at(i).shapeInfo; - if (shapeInfo.points.size() > 0) { - for (int j = 0; j < shapeInfo.points.size(); ++j) { - totalExtents.addPoint(extractTranslation(jointTransform * glm::translate(shapeInfo.points[j]))); - } - } - // HACK so that default legless robot doesn't knuckle-drag - if (shapeInfo.points.size() == 0 && (state.getName() == "LeftFoot" || state.getName() == "RightFoot")) { - totalExtents.addPoint(extractTranslation(jointTransform)); - } - } - - // compute bounding shape parameters - // NOTE: we assume that the longest side of totalExtents is the yAxis... - glm::vec3 diagonal = totalExtents.maximum - totalExtents.minimum; - // ... and assume the radius is half the RMS of the X and Z sides: - _boundingCapsuleRadius = 0.5f * sqrtf(0.5f * (diagonal.x * diagonal.x + diagonal.z * diagonal.z)); - _boundingCapsuleHeight = diagonal.y - 2.0f * _boundingCapsuleRadius; - - glm::vec3 rootPosition = _rig->getJointState(geometry.rootJointIndex).getPosition(); - _boundingCapsuleLocalOffset = 0.5f * (totalExtents.maximum + totalExtents.minimum) - rootPosition; - - // RECOVER FROM BOUNINDG SHAPE HACK: now that we're all done, restore the default pose - for (int i = 0; i < numStates; i++) { - _rig->restoreJointRotation(i, 1.0f, 1.0f); - _rig->restoreJointTranslation(i, 1.0f, 1.0f); - } - */ - - // AJT: REMOVE HARDCODED BOUNDING VOLUME - _boundingCapsuleRadius = 0.3f; - _boundingCapsuleHeight = 1.3f; - _boundingCapsuleLocalOffset = glm::vec3(0.0f, -0.25f, 0.0f); + _rig->computeAvatarBoundingCapsule(geometry, + _boundingCapsuleRadius, + _boundingCapsuleHeight, + _boundingCapsuleLocalOffset); } void SkeletonModel::renderBoundingCollisionShapes(gpu::Batch& batch, float alpha) { diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index f8f7fd9e9e..971d2d5ff1 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -54,9 +54,9 @@ protected: AnimInverseKinematics& operator=(const AnimInverseKinematics&) = delete; struct IKTargetVar { - IKTargetVar(const QString& jointNameIn, - const QString& positionVarIn, - const QString& rotationVarIn, + IKTargetVar(const QString& jointNameIn, + const QString& positionVarIn, + const QString& rotationVarIn, const QString& typeVarIn) : positionVar(positionVarIn), rotationVar(rotationVarIn), diff --git a/libraries/animation/src/AnimSkeleton.cpp b/libraries/animation/src/AnimSkeleton.cpp index ad358709c2..ded8c19671 100644 --- a/libraries/animation/src/AnimSkeleton.cpp +++ b/libraries/animation/src/AnimSkeleton.cpp @@ -96,6 +96,16 @@ AnimPose AnimSkeleton::getAbsolutePose(int jointIndex, const AnimPoseVec& poses) } } +void AnimSkeleton::convertRelativePosesToAbsolute(AnimPoseVec& poses) const { + // poses start off relative and leave in absolute frame + for (int i = 0; i < poses.size() && i < _joints.size(); ++i) { + int parentIndex = _joints[i].parentIndex; + if (parentIndex != -1) { + poses[i] = poses[parentIndex] * poses[i]; + } + } +} + void AnimSkeleton::buildSkeletonFromJoints(const std::vector& joints) { _joints = joints; diff --git a/libraries/animation/src/AnimSkeleton.h b/libraries/animation/src/AnimSkeleton.h index 5e0a80d077..aee9dcda21 100644 --- a/libraries/animation/src/AnimSkeleton.h +++ b/libraries/animation/src/AnimSkeleton.h @@ -51,6 +51,8 @@ public: AnimPose getAbsolutePose(int jointIndex, const AnimPoseVec& poses) const; + void convertRelativePosesToAbsolute(AnimPoseVec& poses) const; + #ifndef NDEBUG void dump() const; void dump(const AnimPoseVec& poses) const; diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 8cd57cd7f1..ae1786f905 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -19,8 +19,9 @@ #include #include "AnimationLogging.h" -#include "AnimSkeleton.h" #include "AnimClip.h" +#include "AnimInverseKinematics.h" +#include "AnimSkeleton.h" #include "IKTarget.h" static bool isEqual(const glm::vec3& u, const glm::vec3& v) { @@ -1065,3 +1066,105 @@ void Rig::copyJointsFromJointData(const QVector& jointDataVec) { setJointTranslation(i, data.translationSet, invGeometryOffset * data.translation, 1.0f); } } + +void Rig::computeAvatarBoundingCapsule( + const FBXGeometry& geometry, + float& radiusOut, + float& heightOut, + glm::vec3& localOffsetOut) const { + if (! _animSkeleton) { + const float DEFAULT_AVATAR_CAPSULE_RADIUS = 0.3f; + const float DEFAULT_AVATAR_CAPSULE_HEIGHT = 1.3f; + const glm::vec3 DEFAULT_AVATAR_CAPSULE_LOCAL_OFFSET = glm::vec3(0.0f, -0.25f, 0.0f); + radiusOut = DEFAULT_AVATAR_CAPSULE_RADIUS; + heightOut = DEFAULT_AVATAR_CAPSULE_HEIGHT; + localOffsetOut = DEFAULT_AVATAR_CAPSULE_LOCAL_OFFSET; + return; + } + + AnimInverseKinematics ikNode("boundingShape"); + ikNode.setSkeleton(_animSkeleton); + ikNode.setTargetVars("LeftHand", + "leftHandPosition", + "leftHandRotation", + "leftHandType"); + ikNode.setTargetVars("RightHand", + "rightHandPosition", + "rightHandRotation", + "rightHandType"); + ikNode.setTargetVars("LeftFoot", + "leftFootPosition", + "leftFootRotation", + "leftFootType"); + ikNode.setTargetVars("RightFoot", + "rightFootPosition", + "rightFootRotation", + "rightFootType"); + + AnimPose geometryToRig = _modelOffset * _geometryOffset; + + AnimPose hips = geometryToRig * _animSkeleton->getAbsoluteBindPose(_animSkeleton->nameToJointIndex("Hips")); + AnimVariantMap animVars; + glm::quat handRotation = glm::angleAxis(PI, Vectors::UNIT_X); + animVars.set("leftHandPosition", hips.trans); + animVars.set("leftHandRotation", handRotation); + animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition); + animVars.set("rightHandPosition", hips.trans); + animVars.set("rightHandRotation", handRotation); + animVars.set("rightHandType", (int)IKTarget::Type::RotationAndPosition); + + int rightFootIndex = _animSkeleton->nameToJointIndex("RightFoot"); + int leftFootIndex = _animSkeleton->nameToJointIndex("LeftFoot"); + if (rightFootIndex != -1 && leftFootIndex != -1) { + glm::vec3 foot = Vectors::ZERO; + glm::quat footRotation = glm::angleAxis(0.5f * PI, Vectors::UNIT_X); + animVars.set("leftFootPosition", foot); + animVars.set("leftFootRotation", footRotation); + animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition); + animVars.set("rightFootPosition", foot); + 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; + float dt = 1.0f; // the value of this does not matter + ikNode.overlay(animVars, dt, triggersOut, _animSkeleton->getRelativeBindPoses()); + AnimPoseVec finalPoses = ikNode.overlay(animVars, dt, triggersOut, _animSkeleton->getRelativeBindPoses()); + + // convert relative poses to absolute + _animSkeleton->convertRelativePosesToAbsolute(finalPoses); + + // compute bounding box that encloses all points + Extents totalExtents; + totalExtents.reset(); + int numPoses = finalPoses.size(); + for (int i = 0; i < numPoses; i++) { + const FBXJointShapeInfo& shapeInfo = geometry.joints.at(i).shapeInfo; + AnimPose pose = finalPoses[i]; + if (shapeInfo.points.size() > 0) { + for (int j = 0; j < shapeInfo.points.size(); ++j) { + totalExtents.addPoint((pose * shapeInfo.points[j])); + } + } else { + totalExtents.addPoint(pose.trans); + } + } + // HACK so that default legless robot doesn't knuckle-drag + if (rightFootIndex > -1) { + totalExtents.addPoint(finalPoses[rightFootIndex].trans); + } + if (leftFootIndex > -1) { + totalExtents.addPoint(finalPoses[leftFootIndex].trans); + } + + // compute bounding shape parameters + // NOTE: we assume that the longest side of totalExtents is the yAxis... + glm::vec3 diagonal = geometryToRig * (totalExtents.maximum - totalExtents.minimum); + // ... and assume the radiusOut is half the RMS of the X and Z sides: + radiusOut = 0.5f * sqrtf(0.5f * (diagonal.x * diagonal.x + diagonal.z * diagonal.z)); + heightOut = diagonal.y - 2.0f * radiusOut; + + glm::vec3 rootPosition = finalPoses[geometry.rootJointIndex].trans; + localOffsetOut = geometryToRig * (0.5f * (totalExtents.maximum + totalExtents.minimum) - rootPosition); +} diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index f1d7395f30..8d1d768c18 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -197,6 +197,8 @@ public: void copyJointsIntoJointData(QVector& jointDataVec) const; void copyJointsFromJointData(const QVector& jointDataVec); + void computeAvatarBoundingCapsule(const FBXGeometry& geometry, float& radiusOut, float& heightOut, glm::vec3& offsetOut) const; + protected: bool isIndexValid(int index) const { return _animSkeleton && index >= 0 && index < _animSkeleton->getNumJoints(); } void updateAnimationStateHandlers();