From c1f777e18fc397ce799b3cbe72a32f5942bdc477 Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Thu, 30 Jul 2015 18:39:01 -0700 Subject: [PATCH 1/2] move setHandPosition from SkeletonModel to AvatarRig. Tell Rig::initJointStates joint-indexes for hands and elbows and shoulders --- interface/src/avatar/SkeletonModel.cpp | 83 ++++++-------------------- interface/src/avatar/SkeletonModel.h | 5 -- libraries/animation/src/AvatarRig.cpp | 65 ++++++++++++++++++++ libraries/animation/src/AvatarRig.h | 2 + libraries/animation/src/EntityRig.h | 2 + libraries/animation/src/Rig.cpp | 18 +++++- libraries/animation/src/Rig.h | 24 +++++++- libraries/render-utils/src/Model.cpp | 18 +++++- 8 files changed, 142 insertions(+), 75 deletions(-) diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index 1b3298c75d..1d3a1df82a 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -42,7 +42,23 @@ SkeletonModel::~SkeletonModel() { void SkeletonModel::initJointStates(QVector states) { const FBXGeometry& geometry = _geometry->getFBXGeometry(); glm::mat4 parentTransform = glm::scale(_scale) * glm::translate(_offset) * geometry.offset; - _boundingRadius = _rig->initJointStates(states, parentTransform); + + int rootJointIndex = geometry.rootJointIndex; + int leftHandJointIndex = geometry.leftHandJointIndex; + int leftElbowJointIndex = leftHandJointIndex >= 0 ? geometry.joints.at(leftHandJointIndex).parentIndex : -1; + int leftShoulderJointIndex = leftElbowJointIndex >= 0 ? geometry.joints.at(leftElbowJointIndex).parentIndex : -1; + int rightHandJointIndex = geometry.rightHandJointIndex; + int rightElbowJointIndex = rightHandJointIndex >= 0 ? geometry.joints.at(rightHandJointIndex).parentIndex : -1; + int rightShoulderJointIndex = rightElbowJointIndex >= 0 ? geometry.joints.at(rightElbowJointIndex).parentIndex : -1; + + _boundingRadius = _rig->initJointStates(states, parentTransform, + rootJointIndex, + leftHandJointIndex, + leftElbowJointIndex, + leftShoulderJointIndex, + rightHandJointIndex, + rightElbowJointIndex, + rightShoulderJointIndex); // Determine the default eye position for avatar scale = 1.0 int headJointIndex = _geometry->getFBXGeometry().headJointIndex; @@ -227,7 +243,7 @@ void SkeletonModel::applyPalmData(int jointIndex, PalmData& palm) { palmRotation = rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), fingerDirection) * palmRotation; if (Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK)) { - setHandPosition(jointIndex, palmPosition, palmRotation); + _rig->setHandPosition(jointIndex, palmPosition, palmRotation, extractUniformScale(_scale), PALM_PRIORITY); } else if (Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) { float forearmLength = geometry.joints.at(jointIndex).distanceToParent * extractUniformScale(_scale); glm::vec3 forearm = palmRotation * glm::vec3(sign * forearmLength, 0.0f, 0.0f); @@ -332,69 +348,6 @@ void SkeletonModel::renderOrientationDirections(gpu::Batch& batch, int jointInde geometryCache->renderLine(batch, position, pFront, blue, jointLineIDs._front); } - - -void SkeletonModel::setHandPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation) { - // this algorithm is from sample code from sixense - const FBXGeometry& geometry = _geometry->getFBXGeometry(); - int elbowJointIndex = geometry.joints.at(jointIndex).parentIndex; - if (elbowJointIndex == -1) { - return; - } - int shoulderJointIndex = geometry.joints.at(elbowJointIndex).parentIndex; - glm::vec3 shoulderPosition; - if (!getJointPosition(shoulderJointIndex, shoulderPosition)) { - return; - } - // precomputed lengths - float scale = extractUniformScale(_scale); - float upperArmLength = geometry.joints.at(elbowJointIndex).distanceToParent * scale; - float lowerArmLength = geometry.joints.at(jointIndex).distanceToParent * scale; - - // first set wrist position - glm::vec3 wristPosition = position; - - glm::vec3 shoulderToWrist = wristPosition - shoulderPosition; - float distanceToWrist = glm::length(shoulderToWrist); - - // prevent gimbal lock - if (distanceToWrist > upperArmLength + lowerArmLength - EPSILON) { - distanceToWrist = upperArmLength + lowerArmLength - EPSILON; - shoulderToWrist = glm::normalize(shoulderToWrist) * distanceToWrist; - wristPosition = shoulderPosition + shoulderToWrist; - } - - // cosine of angle from upper arm to hand vector - float cosA = (upperArmLength * upperArmLength + distanceToWrist * distanceToWrist - lowerArmLength * lowerArmLength) / - (2 * upperArmLength * distanceToWrist); - float mid = upperArmLength * cosA; - float height = sqrt(upperArmLength * upperArmLength + mid * mid - 2 * upperArmLength * mid * cosA); - - // direction of the elbow - glm::vec3 handNormal = glm::cross(rotation * glm::vec3(0.0f, 1.0f, 0.0f), shoulderToWrist); // elbow rotating with wrist - glm::vec3 relaxedNormal = glm::cross(glm::vec3(0.0f, 1.0f, 0.0f), shoulderToWrist); // elbow pointing straight down - const float NORMAL_WEIGHT = 0.5f; - glm::vec3 finalNormal = glm::mix(relaxedNormal, handNormal, NORMAL_WEIGHT); - - bool rightHand = (jointIndex == geometry.rightHandJointIndex); - if (rightHand ? (finalNormal.y > 0.0f) : (finalNormal.y < 0.0f)) { - finalNormal.y = 0.0f; // dont allow elbows to point inward (y is vertical axis) - } - - glm::vec3 tangent = glm::normalize(glm::cross(shoulderToWrist, finalNormal)); - - // ik solution - glm::vec3 elbowPosition = shoulderPosition + glm::normalize(shoulderToWrist) * mid - tangent * height; - glm::vec3 forwardVector(rightHand ? -1.0f : 1.0f, 0.0f, 0.0f); - glm::quat shoulderRotation = rotationBetween(forwardVector, elbowPosition - shoulderPosition); - - _rig->setJointRotationInBindFrame(shoulderJointIndex, shoulderRotation, PALM_PRIORITY); - _rig->setJointRotationInBindFrame(elbowJointIndex, - rotationBetween(shoulderRotation * forwardVector, wristPosition - elbowPosition) * - shoulderRotation, PALM_PRIORITY); - _rig->setJointRotationInBindFrame(jointIndex, rotation, PALM_PRIORITY); -} - bool SkeletonModel::getLeftHandPosition(glm::vec3& position) const { return getJointPositionInWorldFrame(getLeftHandJointIndex(), position); } diff --git a/interface/src/avatar/SkeletonModel.h b/interface/src/avatar/SkeletonModel.h index ecc5c80118..a13d3c4e75 100644 --- a/interface/src/avatar/SkeletonModel.h +++ b/interface/src/avatar/SkeletonModel.h @@ -131,11 +131,6 @@ private: QHash _jointOrientationLines; int _triangleFanID; - /// \param jointIndex index of joint in model - /// \param position position of joint in model-frame - /// \param rotation rotation of joint in model-frame - void setHandPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation); - bool getEyeModelPositions(glm::vec3& firstEyePosition, glm::vec3& secondEyePosition) const; Avatar* _owningAvatar; diff --git a/libraries/animation/src/AvatarRig.cpp b/libraries/animation/src/AvatarRig.cpp index 919ea43e7d..73ac793c80 100644 --- a/libraries/animation/src/AvatarRig.cpp +++ b/libraries/animation/src/AvatarRig.cpp @@ -32,3 +32,68 @@ void AvatarRig::updateJointState(int index, glm::mat4 parentTransform) { } } } + +void AvatarRig::setHandPosition(int jointIndex, + const glm::vec3& position, const glm::quat& rotation, + float scale, float priority) { + bool rightHand = (jointIndex == _rightHandJointIndex); + + int elbowJointIndex = rightHand ? _rightElbowJointIndex : _leftElbowJointIndex; + int shoulderJointIndex = rightHand ? _rightShoulderJointIndex : _leftShoulderJointIndex; + + // this algorithm is from sample code from sixense + if (elbowJointIndex == -1 || shoulderJointIndex == -1) { + return; + } + + glm::vec3 shoulderPosition; + if (!getJointPosition(shoulderJointIndex, shoulderPosition)) { + return; + } + + // precomputed lengths + float upperArmLength = _jointStates[elbowJointIndex].getFBXJoint().distanceToParent * scale; + float lowerArmLength = _jointStates[jointIndex].getFBXJoint().distanceToParent * scale; + + // first set wrist position + glm::vec3 wristPosition = position; + + glm::vec3 shoulderToWrist = wristPosition - shoulderPosition; + float distanceToWrist = glm::length(shoulderToWrist); + + // prevent gimbal lock + if (distanceToWrist > upperArmLength + lowerArmLength - EPSILON) { + distanceToWrist = upperArmLength + lowerArmLength - EPSILON; + shoulderToWrist = glm::normalize(shoulderToWrist) * distanceToWrist; + wristPosition = shoulderPosition + shoulderToWrist; + } + + // cosine of angle from upper arm to hand vector + float cosA = (upperArmLength * upperArmLength + distanceToWrist * distanceToWrist - lowerArmLength * lowerArmLength) / + (2 * upperArmLength * distanceToWrist); + float mid = upperArmLength * cosA; + float height = sqrt(upperArmLength * upperArmLength + mid * mid - 2 * upperArmLength * mid * cosA); + + // direction of the elbow + glm::vec3 handNormal = glm::cross(rotation * glm::vec3(0.0f, 1.0f, 0.0f), shoulderToWrist); // elbow rotating with wrist + glm::vec3 relaxedNormal = glm::cross(glm::vec3(0.0f, 1.0f, 0.0f), shoulderToWrist); // elbow pointing straight down + const float NORMAL_WEIGHT = 0.5f; + glm::vec3 finalNormal = glm::mix(relaxedNormal, handNormal, NORMAL_WEIGHT); + + if (rightHand ? (finalNormal.y > 0.0f) : (finalNormal.y < 0.0f)) { + finalNormal.y = 0.0f; // dont allow elbows to point inward (y is vertical axis) + } + + glm::vec3 tangent = glm::normalize(glm::cross(shoulderToWrist, finalNormal)); + + // ik solution + glm::vec3 elbowPosition = shoulderPosition + glm::normalize(shoulderToWrist) * mid - tangent * height; + glm::vec3 forwardVector(rightHand ? -1.0f : 1.0f, 0.0f, 0.0f); + glm::quat shoulderRotation = rotationBetween(forwardVector, elbowPosition - shoulderPosition); + + setJointRotationInBindFrame(shoulderJointIndex, shoulderRotation, priority); + setJointRotationInBindFrame(elbowJointIndex, + rotationBetween(shoulderRotation * forwardVector, wristPosition - elbowPosition) * + shoulderRotation, priority); + setJointRotationInBindFrame(jointIndex, rotation, priority); +} diff --git a/libraries/animation/src/AvatarRig.h b/libraries/animation/src/AvatarRig.h index 4a111a535b..5e2153e226 100644 --- a/libraries/animation/src/AvatarRig.h +++ b/libraries/animation/src/AvatarRig.h @@ -22,6 +22,8 @@ class AvatarRig : public Rig { public: ~AvatarRig() {} virtual void updateJointState(int index, glm::mat4 parentTransform); + virtual void setHandPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation, + float scale, float priority); }; #endif // hifi_AvatarRig_h diff --git a/libraries/animation/src/EntityRig.h b/libraries/animation/src/EntityRig.h index e8e15a5a28..9b519a7bfe 100644 --- a/libraries/animation/src/EntityRig.h +++ b/libraries/animation/src/EntityRig.h @@ -22,6 +22,8 @@ class EntityRig : public Rig { public: ~EntityRig() {} virtual void updateJointState(int index, glm::mat4 parentTransform); + virtual void setHandPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation, + float scale, float priority) {} }; #endif // hifi_EntityRig_h diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 15f3dd65ec..e986d220d7 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -143,8 +143,24 @@ void Rig::deleteAnimations() { _animationHandles.clear(); } -float Rig::initJointStates(QVector states, glm::mat4 parentTransform) { +float Rig::initJointStates(QVector states, glm::mat4 parentTransform, + int rootJointIndex, + int leftHandJointIndex, + int leftElbowJointIndex, + int leftShoulderJointIndex, + int rightHandJointIndex, + int rightElbowJointIndex, + int rightShoulderJointIndex) { _jointStates = states; + + _rootJointIndex = rootJointIndex; + _leftHandJointIndex = leftHandJointIndex; + _leftElbowJointIndex = leftElbowJointIndex; + _leftShoulderJointIndex = leftShoulderJointIndex; + _rightHandJointIndex = rightHandJointIndex; + _rightElbowJointIndex = rightElbowJointIndex; + _rightShoulderJointIndex = rightShoulderJointIndex; + initJointTransforms(parentTransform); int numStates = _jointStates.size(); diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 2af61bfded..533717a422 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -42,7 +42,6 @@ class AnimationHandle; typedef std::shared_ptr AnimationHandlePointer; -// typedef QWeakPointer WeakAnimationHandlePointer; class Rig; typedef std::shared_ptr RigPointer; @@ -90,7 +89,14 @@ public: float priority = 1.0f, bool loop = false, bool hold = false, float firstFrame = 0.0f, float lastFrame = FLT_MAX, const QStringList& maskedJoints = QStringList(), bool startAutomatically = false); - float initJointStates(QVector states, glm::mat4 parentTransform); + float initJointStates(QVector states, glm::mat4 parentTransform, + int rootJointIndex, + int leftHandJointIndex, + int leftElbowJointIndex, + int leftShoulderJointIndex, + int rightHandJointIndex, + int rightElbowJointIndex, + int rightShoulderJointIndex); bool jointStatesEmpty() { return _jointStates.isEmpty(); }; int getJointStateCount() const { return _jointStates.size(); } int indexOfJoint(const QString& jointName) ; @@ -149,6 +155,9 @@ public: void updateFromHeadParameters(const HeadParameters& params); + virtual void setHandPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation, + float scale, float priority) = 0; + protected: void updateLeanJoint(int index, float leanSideways, float leanForward, float torsoTwist); @@ -156,6 +165,15 @@ public: void updateEyeJoint(int index, const glm::quat& worldHeadOrientation, const glm::vec3& lookAt, const glm::vec3& saccade); QVector _jointStates; + int _rootJointIndex = -1; + + int _leftHandJointIndex = -1; + int _leftElbowJointIndex = -1; + int _leftShoulderJointIndex = -1; + + int _rightHandJointIndex = -1; + int _rightElbowJointIndex = -1; + int _rightShoulderJointIndex = -1; QList _animationHandles; QList _runningAnimations; @@ -166,6 +184,6 @@ public: bool _isIdle; glm::vec3 _lastFront; glm::vec3 _lastPosition; - }; +}; #endif /* defined(__hifi__Rig__) */ diff --git a/libraries/render-utils/src/Model.cpp b/libraries/render-utils/src/Model.cpp index 417a4bb16f..d89c40a0be 100644 --- a/libraries/render-utils/src/Model.cpp +++ b/libraries/render-utils/src/Model.cpp @@ -474,7 +474,23 @@ bool Model::updateGeometry() { void Model::initJointStates(QVector states) { const FBXGeometry& geometry = _geometry->getFBXGeometry(); glm::mat4 parentTransform = glm::scale(_scale) * glm::translate(_offset) * geometry.offset; - _boundingRadius = _rig->initJointStates(states, parentTransform); + + int rootJointIndex = geometry.rootJointIndex; + int leftHandJointIndex = geometry.leftHandJointIndex; + int leftElbowJointIndex = leftHandJointIndex >= 0 ? geometry.joints.at(leftHandJointIndex).parentIndex : -1; + int leftShoulderJointIndex = leftElbowJointIndex >= 0 ? geometry.joints.at(leftElbowJointIndex).parentIndex : -1; + int rightHandJointIndex = geometry.rightHandJointIndex; + int rightElbowJointIndex = rightHandJointIndex >= 0 ? geometry.joints.at(rightHandJointIndex).parentIndex : -1; + int rightShoulderJointIndex = rightElbowJointIndex >= 0 ? geometry.joints.at(rightElbowJointIndex).parentIndex : -1; + + _boundingRadius = _rig->initJointStates(states, parentTransform, + rootJointIndex, + leftHandJointIndex, + leftElbowJointIndex, + leftShoulderJointIndex, + rightHandJointIndex, + rightElbowJointIndex, + rightShoulderJointIndex); } bool Model::findRayIntersectionAgainstSubMeshes(const glm::vec3& origin, const glm::vec3& direction, float& distance, From 241e767ab6eca0558679f59ba9c37b056346bc0a Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Thu, 30 Jul 2015 18:44:09 -0700 Subject: [PATCH 2/2] move setHandPosition from SkeletonModel to AvatarRig. Tell Rig::initJointStates joint-indexes for hands and elbows and shoulders --- libraries/animation/src/AvatarRig.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/libraries/animation/src/AvatarRig.cpp b/libraries/animation/src/AvatarRig.cpp index 73ac793c80..97fbb82944 100644 --- a/libraries/animation/src/AvatarRig.cpp +++ b/libraries/animation/src/AvatarRig.cpp @@ -20,13 +20,14 @@ void AvatarRig::updateJointState(int index, glm::mat4 parentTransform) { const FBXJoint& joint = state.getFBXJoint(); // compute model transforms - int parentIndex = joint.parentIndex; - if (parentIndex == -1) { + if (index == _rootJointIndex) { + // we always zero-out the translation part of an avatar's root join-transform. state.computeTransform(parentTransform); clearJointTransformTranslation(index); } else { // guard against out-of-bounds access to _jointStates - if (joint.parentIndex >= 0 && joint.parentIndex < _jointStates.size()) { + int parentIndex = joint.parentIndex; + if (parentIndex >= 0 && parentIndex < _jointStates.size()) { const JointState& parentState = _jointStates.at(parentIndex); state.computeTransform(parentState.getTransform(), parentState.getTransformChanged()); }