From e7f32c211bfb3ea802cc13c2673d886ab50852e4 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Tue, 3 Jun 2014 14:48:57 -0700 Subject: [PATCH] remove world-frame hand/palm set methods --- interface/src/avatar/SkeletonModel.cpp | 135 +------------------------ interface/src/avatar/SkeletonModel.h | 3 - interface/src/renderer/Model.cpp | 94 +---------------- interface/src/renderer/Model.h | 5 +- 4 files changed, 5 insertions(+), 232 deletions(-) diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index 8a3082d89a..4c2c80d11a 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -70,7 +70,7 @@ void SkeletonModel::simulate(float deltaTime, bool fullUpdate) { } else if (leftPalmIndex == rightPalmIndex) { // right hand only - applyPalmData(geometry.rightHandJointIndex, hand->getPalms()[leftPalmIndex]); + applyPalmDataInModelFrame(geometry.rightHandJointIndex, hand->getPalms()[leftPalmIndex]); restoreLeftHandPosition(HAND_RESTORATION_RATE, PALM_PRIORITY); } else { @@ -132,29 +132,6 @@ bool operator<(const IndexValue& firstIndex, const IndexValue& secondIndex) { return firstIndex.value < secondIndex.value; } -void SkeletonModel::applyHandPosition(int jointIndex, const glm::vec3& position) { - if (jointIndex == -1 || jointIndex >= _jointStates.size()) { - return; - } - setJointPosition(jointIndex, position, glm::quat(), false, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY); - - const FBXGeometry& geometry = _geometry->getFBXGeometry(); - glm::vec3 handPosition, elbowPosition; - getJointPosition(jointIndex, handPosition); - getJointPosition(geometry.joints.at(jointIndex).parentIndex, elbowPosition); - glm::vec3 forearmVector = handPosition - elbowPosition; - float forearmLength = glm::length(forearmVector); - if (forearmLength < EPSILON) { - return; - } - JointState& state = _jointStates[jointIndex]; - glm::quat handRotation = state.getJointRotation(); - - // align hand with forearm - float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f; - state.applyRotationDelta(rotationBetween(handRotation * glm::vec3(-sign, 0.0f, 0.0f), forearmVector), true, PALM_PRIORITY); -} - void SkeletonModel::applyHandPositionInModelFrame(int jointIndex, const glm::vec3& position) { if (jointIndex == -1 || jointIndex >= _jointStates.size()) { return; @@ -178,52 +155,6 @@ void SkeletonModel::applyHandPositionInModelFrame(int jointIndex, const glm::vec state.applyRotationDeltaInModelFrame(rotationBetween(handRotation * glm::vec3(-sign, 0.0f, 0.0f), forearmVector), true, PALM_PRIORITY); } -void SkeletonModel::applyPalmData(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; - if (!Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK) && - Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) { - JointState parentState = _jointStates[parentJointIndex]; - palmRotation = parentState.getJointRotation(); - } else { - JointState state = _jointStates[jointIndex]; - palmRotation = state.getJointRotation(); - } - palmRotation = rotationBetween(palmRotation * geometry.palmDirection, palm.getNormal()) * palmRotation; - - // rotate palm to align with finger direction - glm::vec3 direction = palm.getFingerDirection(); - palmRotation = rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction) * palmRotation; - - // set hand position, rotation - if (Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK)) { - setHandPosition(jointIndex, palm.getPosition(), palmRotation); - - } else if (Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) { - glm::vec3 forearmVector = palmRotation * glm::vec3(sign, 0.0f, 0.0f); - setJointPosition(parentJointIndex, palm.getPosition() + 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.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; @@ -377,70 +308,6 @@ void SkeletonModel::renderJointConstraints(int jointIndex) { glLineWidth(1.0f); } -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); - - JointState& shoulderState = _jointStates[shoulderJointIndex]; - shoulderState.setRotation(shoulderRotation, PALM_PRIORITY); - - JointState& elbowState = _jointStates[elbowJointIndex]; - elbowState.setRotation(rotationBetween(shoulderRotation * forwardVector, wristPosition - elbowPosition) * shoulderRotation, PALM_PRIORITY); - - 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(); diff --git a/interface/src/avatar/SkeletonModel.h b/interface/src/avatar/SkeletonModel.h index 5447027d96..092722fd40 100644 --- a/interface/src/avatar/SkeletonModel.h +++ b/interface/src/avatar/SkeletonModel.h @@ -91,10 +91,8 @@ public: protected: - void applyHandPosition(int jointIndex, const glm::vec3& position); void applyHandPositionInModelFrame(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. @@ -107,7 +105,6 @@ protected: 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 2616ded481..65c9f1e691 100644 --- a/interface/src/renderer/Model.cpp +++ b/interface/src/renderer/Model.cpp @@ -1288,95 +1288,6 @@ void Model::updateJointState(int index) { } } -bool Model::setJointPosition(int jointIndex, const glm::vec3& translation, const glm::quat& rotation, bool useRotation, - int lastFreeIndex, bool allIntermediatesFree, const glm::vec3& alignment, float priority) { - if (jointIndex == -1 || _jointStates.isEmpty()) { - return false; - } - glm::vec3 relativePosition = translation - _translation; - 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 = _rotation * 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].getHybridTransform()); - 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.getHybridTransform()); - glm::vec3 jointVector = endPosition - jointPosition; - glm::quat oldCombinedRotation = _rotation * state.getRotationInModelFrame(); - glm::quat combinedDelta; - float combinedWeight; - if (useRotation) { - combinedDelta = safeMix(rotation * glm::inverse(endRotation), - rotationBetween(jointVector, relativePosition - jointPosition), 0.5f); - combinedWeight = 2.0f; - - } else { - combinedDelta = rotationBetween(jointVector, relativePosition - 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).getHybridTransform()); - } - 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.applyRotationDelta(combinedDelta, true, priority); - glm::quat actualDelta = _rotation * 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::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()) { @@ -1614,9 +1525,10 @@ void Model::applyCollision(CollisionInfo& collision) { axis = glm::normalize(axis); glm::vec3 end; getJointPosition(jointIndex, end); - glm::vec3 newEnd = start + glm::angleAxis(angle, axis) * (end - start); + // transform into model-frame + glm::vec3 newEnd = glm::inverse(_rotation) * (start + glm::angleAxis(angle, axis) * (end - start) - _translation); // try to move it - setJointPosition(jointIndex, newEnd, glm::quat(), false, -1, true); + setJointPositionInModelFrame(jointIndex, newEnd, glm::quat(), false, -1, true); } } } diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h index 37f0ffbc60..31d59d7c3e 100644 --- a/interface/src/renderer/Model.h +++ b/interface/src/renderer/Model.h @@ -176,6 +176,7 @@ public: int getLastFreeJointIndex(int jointIndex) const; bool getJointPosition(int jointIndex, glm::vec3& position) const; + bool getJointRotationInWorldFrame(int jointIndex, glm::quat& rotation) const; bool getJointCombinedRotation(int jointIndex, glm::quat& rotation) const; @@ -268,10 +269,6 @@ protected: /// Updates the state of the joint at the specified index. virtual void updateJointState(int index); - 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);