From 03bf1fe69df74167d514e62de551a572589457bf Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 3 Jun 2014 14:04:09 -0700 Subject: [PATCH] Add joint manipulations in model-frame --- interface/src/avatar/SkeletonModel.cpp | 121 ++++++++++++++++++++++++- interface/src/avatar/SkeletonModel.h | 2 + interface/src/renderer/Model.cpp | 112 ++++++++++++++++++++++- interface/src/renderer/Model.h | 9 ++ 4 files changed, 238 insertions(+), 6 deletions(-) diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index 48efbdc4dc..1f2f502b44 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -73,8 +73,8 @@ void SkeletonModel::simulate(float deltaTime, bool fullUpdate) { restoreLeftHandPosition(HAND_RESTORATION_RATE, PALM_PRIORITY); } else { - applyPalmData(geometry.leftHandJointIndex, hand->getPalms()[leftPalmIndex]); - applyPalmData(geometry.rightHandJointIndex, hand->getPalms()[rightPalmIndex]); + applyPalmDataInModelFrame(geometry.leftHandJointIndex, hand->getPalms()[leftPalmIndex]); + applyPalmDataInModelFrame(geometry.rightHandJointIndex, hand->getPalms()[rightPalmIndex]); } } @@ -194,13 +194,64 @@ void SkeletonModel::applyPalmData(int jointIndex, PalmData& palm) { parentState.setRotation(palmRotation, PALM_PRIORITY); // slam parent-relative rotation to identity _jointStates[jointIndex]._rotation = glm::quat(); - } else { setJointPosition(jointIndex, palm.getPosition(), palmRotation, true, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY); } } +void SkeletonModel::applyPalmDataInModelFrame(int jointIndex, PalmData& palm) { + if (jointIndex == -1 || jointIndex >= _jointStates.size()) { + return; + } + const FBXGeometry& geometry = _geometry->getFBXGeometry(); + float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f; + int parentJointIndex = geometry.joints.at(jointIndex).parentIndex; + if (parentJointIndex == -1) { + return; + } + + // rotate palm to align with its normal (normal points out of hand's palm) + glm::quat palmRotation; + glm::quat r0, r1; + if (!Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK) && + Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) { + JointState parentState = _jointStates[parentJointIndex]; + palmRotation = parentState.getRotationFromBindToModelFrame(); + r0 = palmRotation; + } else { + JointState state = _jointStates[jointIndex]; + palmRotation = state.getRotationFromBindToModelFrame(); + } + glm::quat inverseRotation = glm::inverse(_rotation); + glm::vec3 palmNormal = inverseRotation * palm.getNormal(); + palmRotation = rotationBetween(palmRotation * geometry.palmDirection, palmNormal) * palmRotation; + r1 = palmRotation; + + // rotate palm to align with finger direction + glm::vec3 direction = inverseRotation * palm.getFingerDirection(); + palmRotation = rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction) * palmRotation; + + // set hand position, rotation + glm::vec3 palmPosition = inverseRotation * (palm.getPosition() - _translation); + if (Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK)) { + setHandPositionInModelFrame(jointIndex, palmPosition, palmRotation); + + } else if (Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) { + glm::vec3 forearmVector = palmRotation * glm::vec3(sign, 0.0f, 0.0f); + setJointPositionInModelFrame(parentJointIndex, palmPosition + forearmVector * + geometry.joints.at(jointIndex).distanceToParent * extractUniformScale(_scale), + glm::quat(), false, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY); + JointState& parentState = _jointStates[parentJointIndex]; + parentState.setRotationInModelFrame(palmRotation, PALM_PRIORITY); + // slam parent-relative rotation to identity + _jointStates[jointIndex]._rotation = glm::quat(); + } else { + setJointPositionInModelFrame(jointIndex, palmPosition, palmRotation, + true, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY); + } +} + void SkeletonModel::updateJointState(int index) { JointState& state = _jointStates[index]; const FBXJoint& joint = state.getFBXJoint(); @@ -365,6 +416,70 @@ void SkeletonModel::setHandPosition(int jointIndex, const glm::vec3& position, c JointState& handState = _jointStates[jointIndex]; handState.setRotation(rotation, PALM_PRIORITY); } + +void SkeletonModel::setHandPositionInModelFrame(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 (!getJointPositionInModelFrame(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); + + JointState& shoulderState = _jointStates[shoulderJointIndex]; + shoulderState.setRotationInModelFrame(shoulderRotation, PALM_PRIORITY); + + JointState& elbowState = _jointStates[elbowJointIndex]; + elbowState.setRotationInModelFrame(rotationBetween(shoulderRotation * forwardVector, wristPosition - elbowPosition) * shoulderRotation, PALM_PRIORITY); + + JointState& handState = _jointStates[jointIndex]; + handState.setRotationInModelFrame(rotation, PALM_PRIORITY); +} bool SkeletonModel::getLeftHandPosition(glm::vec3& position) const { return getJointPosition(getLeftHandJointIndex(), position); diff --git a/interface/src/avatar/SkeletonModel.h b/interface/src/avatar/SkeletonModel.h index 91070e4ad6..183ba899b8 100644 --- a/interface/src/avatar/SkeletonModel.h +++ b/interface/src/avatar/SkeletonModel.h @@ -94,6 +94,7 @@ protected: void applyHandPosition(int jointIndex, const glm::vec3& position); void applyPalmData(int jointIndex, PalmData& palm); + void applyPalmDataInModelFrame(int jointIndex, PalmData& palm); /// Updates the state of the joint at the specified index. virtual void updateJointState(int index); @@ -106,6 +107,7 @@ private: void renderJointConstraints(int jointIndex); void setHandPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation); + void setHandPositionInModelFrame(int jointIndex, const glm::vec3& position, const glm::quat& rotation); Avatar* _owningAvatar; }; diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp index e3be11c5d2..2616ded481 100644 --- a/interface/src/renderer/Model.cpp +++ b/interface/src/renderer/Model.cpp @@ -756,6 +756,14 @@ bool Model::getJointPosition(int jointIndex, glm::vec3& position) const { return true; } +bool Model::getJointPositionInModelFrame(int jointIndex, glm::vec3& position) const { + if (jointIndex == -1 || jointIndex >= _jointStates.size()) { + return false; + } + position = extractTranslation(_jointStates[jointIndex].getTransformInModelFrame()); + return true; +} + bool Model::getJointRotationInWorldFrame(int jointIndex, glm::quat& rotation) const { if (jointIndex == -1 || jointIndex >= _jointStates.size()) { return false; @@ -1369,6 +1377,94 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& translation, const return true; } +bool Model::setJointPositionInModelFrame(int jointIndex, const glm::vec3& position, const glm::quat& rotation, bool useRotation, + int lastFreeIndex, bool allIntermediatesFree, const glm::vec3& alignment, float priority) { + if (jointIndex == -1 || _jointStates.isEmpty()) { + return false; + } + const FBXGeometry& geometry = _geometry->getFBXGeometry(); + const QVector& freeLineage = geometry.joints.at(jointIndex).freeLineage; + if (freeLineage.isEmpty()) { + return false; + } + if (lastFreeIndex == -1) { + lastFreeIndex = freeLineage.last(); + } + + // this is a cyclic coordinate descent algorithm: see + // http://www.ryanjuckett.com/programming/animation/21-cyclic-coordinate-descent-in-2d + const int ITERATION_COUNT = 1; + glm::vec3 worldAlignment = alignment; + for (int i = 0; i < ITERATION_COUNT; i++) { + // first, try to rotate the end effector as close as possible to the target rotation, if any + glm::quat endRotation; + if (useRotation) { + JointState& state = _jointStates[jointIndex]; + + // TODO: figure out what this is trying to do and combine it into one JointState method + endRotation = state.getJointRotation(); + state.applyRotationDelta(rotation * glm::inverse(endRotation), true, priority); + endRotation = state.getJointRotation(); + } + + // then, we go from the joint upwards, rotating the end as close as possible to the target + glm::vec3 endPosition = extractTranslation(_jointStates[jointIndex].getTransformInModelFrame()); + for (int j = 1; freeLineage.at(j - 1) != lastFreeIndex; j++) { + int index = freeLineage.at(j); + JointState& state = _jointStates[index]; + const FBXJoint& joint = state.getFBXJoint(); + if (!(joint.isFree || allIntermediatesFree)) { + continue; + } + glm::vec3 jointPosition = extractTranslation(state.getTransformInModelFrame()); + glm::vec3 jointVector = endPosition - jointPosition; + glm::quat oldCombinedRotation = state.getRotationInModelFrame(); + glm::quat combinedDelta; + float combinedWeight; + if (useRotation) { + combinedDelta = safeMix(rotation * glm::inverse(endRotation), + rotationBetween(jointVector, position - jointPosition), 0.5f); + combinedWeight = 2.0f; + + } else { + combinedDelta = rotationBetween(jointVector, position - jointPosition); + combinedWeight = 1.0f; + } + if (alignment != glm::vec3() && j > 1) { + jointVector = endPosition - jointPosition; + glm::vec3 positionSum; + for (int k = j - 1; k > 0; k--) { + int index = freeLineage.at(k); + updateJointState(index); + positionSum += extractTranslation(_jointStates.at(index).getTransformInModelFrame()); + } + glm::vec3 projectedCenterOfMass = glm::cross(jointVector, + glm::cross(positionSum / (j - 1.0f) - jointPosition, jointVector)); + glm::vec3 projectedAlignment = glm::cross(jointVector, glm::cross(worldAlignment, jointVector)); + const float LENGTH_EPSILON = 0.001f; + if (glm::length(projectedCenterOfMass) > LENGTH_EPSILON && glm::length(projectedAlignment) > LENGTH_EPSILON) { + combinedDelta = safeMix(combinedDelta, rotationBetween(projectedCenterOfMass, projectedAlignment), + 1.0f / (combinedWeight + 1.0f)); + } + } + state.applyRotationDeltaInModelFrame(combinedDelta, true, priority); + glm::quat actualDelta = state.getRotationInModelFrame() * glm::inverse(oldCombinedRotation); + endPosition = actualDelta * jointVector + jointPosition; + if (useRotation) { + endRotation = actualDelta * endRotation; + } + } + } + + // now update the joint states from the top + for (int j = freeLineage.size() - 1; j >= 0; j--) { + updateJointState(freeLineage.at(j)); + } + _shapesAreDirty = true; + + return true; +} + bool Model::restoreJointPosition(int jointIndex, float fraction, float priority) { if (jointIndex == -1 || _jointStates.isEmpty()) { return false; @@ -2072,6 +2168,10 @@ glm::quat JointState::getJointRotation() const { return _combinedRotation * _fbxJoint->inverseBindRotation; } +glm::quat JointState::getRotationFromBindToModelFrame() const { + return _rotationInModelFrame * _fbxJoint->inverseBindRotation; +} + void JointState::restoreRotation(float fraction, float priority) { assert(_fbxJoint != NULL); if (priority == _animationPriority) { @@ -2088,6 +2188,14 @@ void JointState::setRotation(const glm::quat& rotation, float priority) { } } +void JointState::setRotationInModelFrame(const glm::quat& rotation, float priority) { + assert(_fbxJoint != NULL); + if (priority >= _animationPriority) { + _rotation = _rotation * glm::inverse(_rotationInModelFrame) * rotation * glm::inverse(_fbxJoint->inverseBindRotation); + _animationPriority = priority; + } +} + void JointState::clearTransformTranslation() { _transform[3][0] = 0.0f; _transform[3][1] = 0.0f; @@ -2117,8 +2225,7 @@ void JointState::applyRotationDelta(const glm::quat& delta, bool constrain, floa _rotation = newRotation; } -/* -void JointState::applyRotationDelta(const glm::quat& delta, bool constrain, float priority) { +void JointState::applyRotationDeltaInModelFrame(const glm::quat& delta, bool constrain, float priority) { assert(_fbxJoint != NULL); if (priority < _animationPriority) { return; @@ -2137,7 +2244,6 @@ void JointState::applyRotationDelta(const glm::quat& delta, bool constrain, floa _rotationInModelFrame = _rotationInModelFrame * glm::inverse(_rotation) * newRotation; _rotation = newRotation; } -*/ const glm::vec3& JointState::getDefaultTranslationInParentFrame() const { assert(_fbxJoint != NULL); diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h index b6ae6c9012..37f0ffbc60 100644 --- a/interface/src/renderer/Model.h +++ b/interface/src/renderer/Model.h @@ -54,8 +54,10 @@ public: /// \return rotation from the joint's default (or bind) frame to world frame glm::quat getJointRotation() const; + glm::quat getRotationFromBindToModelFrame() const; void applyRotationDelta(const glm::quat& delta, bool constrain = true, float priority = 1.0f); + void applyRotationDeltaInModelFrame(const glm::quat& delta, bool constrain = true, float priority = 1.0f); const glm::vec3& getDefaultTranslationInParentFrame() const; @@ -65,6 +67,7 @@ public: /// computes parent relative _rotation and sets that /// \warning no combined transforms are updated! void setRotation(const glm::quat& rotation, float priority); + void setRotationInModelFrame(const glm::quat& rotation, float priority); const glm::mat4& getHybridTransform() const { return _transform; } //const glm::quat& getRotationInWorldFrame() const { return _combinedRotation; } @@ -176,6 +179,8 @@ public: bool getJointRotationInWorldFrame(int jointIndex, glm::quat& rotation) const; bool getJointCombinedRotation(int jointIndex, glm::quat& rotation) const; + bool getJointPositionInModelFrame(int jointIndex, glm::vec3& position) const; + QStringList getJointNames() const; AnimationHandlePointer createAnimationHandle(); @@ -266,6 +271,10 @@ protected: bool setJointPosition(int jointIndex, const glm::vec3& translation, const glm::quat& rotation = glm::quat(), bool useRotation = false, int lastFreeIndex = -1, bool allIntermediatesFree = false, const glm::vec3& alignment = glm::vec3(0.0f, -1.0f, 0.0f), float priority = 1.0f); + + bool setJointPositionInModelFrame(int jointIndex, const glm::vec3& translation, const glm::quat& rotation = glm::quat(), + bool useRotation = false, int lastFreeIndex = -1, bool allIntermediatesFree = false, + const glm::vec3& alignment = glm::vec3(0.0f, -1.0f, 0.0f), float priority = 1.0f); /// Restores the indexed joint to its default position. /// \param fraction the fraction of the default position to apply (i.e., 0.25f to slerp one fourth of the way to