From 8388255542bf7a340930a64e5a29ed857a520730 Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Wed, 14 May 2014 15:29:16 -0700 Subject: [PATCH 1/4] Working on alternate IK mode. --- interface/src/Menu.cpp | 1 + interface/src/Menu.h | 1 + interface/src/avatar/SkeletonModel.cpp | 9 ++++++++- interface/src/avatar/SkeletonModel.h | 1 + 4 files changed, 11 insertions(+), 1 deletion(-) diff --git a/interface/src/Menu.cpp b/interface/src/Menu.cpp index 2daf5b0240..7c8f1d75a9 100644 --- a/interface/src/Menu.cpp +++ b/interface/src/Menu.cpp @@ -351,6 +351,7 @@ Menu::Menu() : addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::HandsCollideWithSelf, 0, false); addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::ShowIKConstraints, 0, false); addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::AlignForearmsWithWrists, 0, true); + addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::AlternateIK, 0, false); addDisabledActionAndSeparator(developerMenu, "Testing"); diff --git a/interface/src/Menu.h b/interface/src/Menu.h index 230584bf07..2a521bf59c 100644 --- a/interface/src/Menu.h +++ b/interface/src/Menu.h @@ -267,6 +267,7 @@ private: namespace MenuOption { const QString AboutApp = "About Interface"; const QString AlignForearmsWithWrists = "Align Forearms with Wrists"; + const QString AlternateIK = "Alternate IK"; const QString AmbientOcclusion = "Ambient Occlusion"; const QString Atmosphere = "Atmosphere"; const QString Attachments = "Attachments..."; diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index e48ebfa63c..1e57481c66 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -176,7 +176,10 @@ void SkeletonModel::applyPalmData(int jointIndex, PalmData& palm) { palmRotation = rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction) * palmRotation; // set hand position, rotation - if (Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) { + 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)); @@ -276,3 +279,7 @@ void SkeletonModel::renderJointConstraints(int jointIndex) { glLineWidth(1.0f); } +void SkeletonModel::setHandPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation) { + +} + diff --git a/interface/src/avatar/SkeletonModel.h b/interface/src/avatar/SkeletonModel.h index 20384829ea..77e6ea33d4 100644 --- a/interface/src/avatar/SkeletonModel.h +++ b/interface/src/avatar/SkeletonModel.h @@ -51,6 +51,7 @@ protected: private: void renderJointConstraints(int jointIndex); + void setHandPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation); Avatar* _owningAvatar; }; From 7bee0478be2f53fdb8016f5576c0a45fa3d458e7 Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Thu, 15 May 2014 11:30:09 -0700 Subject: [PATCH 2/4] More work on integrating IK solution from Sixense. --- interface/src/avatar/SkeletonModel.cpp | 52 +++++++++++++++++++++++++- 1 file changed, 51 insertions(+), 1 deletion(-) diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index 1e57481c66..1921094a0f 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -164,7 +164,8 @@ void SkeletonModel::applyPalmData(int jointIndex, PalmData& palm) { // rotate palm to align with its normal (normal points out of hand's palm) glm::quat palmRotation; - if (Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) { + if (!Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK) && + Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) { getJointRotation(parentJointIndex, palmRotation, true); } else { getJointRotation(jointIndex, palmRotation, true); @@ -280,6 +281,55 @@ void SkeletonModel::renderJointConstraints(int jointIndex) { } 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); + + if((jointIndex == geometry.rightHandJointIndex && finalNormal.y > 0.0f) || + (jointIndex == geometry.leftHandJointIndex && finalNormal.y < 0)) { + 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; } From 3eefb6a93ee73d45c1f0ed4cfa1dd2078fd54b9b Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Thu, 15 May 2014 15:32:32 -0700 Subject: [PATCH 3/4] Rotation bits for alternate IK. --- interface/src/avatar/SkeletonModel.cpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index 1921094a0f..540565f5b8 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -322,8 +322,8 @@ void SkeletonModel::setHandPosition(int jointIndex, const glm::vec3& position, c const float NORMAL_WEIGHT = 0.5f; glm::vec3 finalNormal = glm::mix(relaxedNormal, handNormal, NORMAL_WEIGHT); - if((jointIndex == geometry.rightHandJointIndex && finalNormal.y > 0.0f) || - (jointIndex == geometry.leftHandJointIndex && finalNormal.y < 0)) { + 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) } @@ -331,5 +331,17 @@ void SkeletonModel::setHandPosition(int jointIndex, const glm::vec3& position, c // 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; + getJointRotation(shoulderJointIndex, shoulderRotation, true); + applyRotationDelta(shoulderJointIndex, rotationBetween(shoulderRotation * forwardVector, elbowPosition - shoulderPosition), false); + + glm::quat elbowRotation; + getJointRotation(elbowJointIndex, elbowRotation, true); + applyRotationDelta(elbowJointIndex, rotationBetween(elbowRotation * forwardVector, wristPosition - elbowPosition), false); + + setJointRotation(jointIndex, rotation, true); } From 301234a39773208192066d0903ea4836fbf79bc4 Mon Sep 17 00:00:00 2001 From: Andrzej Kapolka Date: Thu, 15 May 2014 15:34:01 -0700 Subject: [PATCH 4/4] Tabs -> spaces. --- interface/src/avatar/SkeletonModel.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index 540565f5b8..3786280a3c 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -320,16 +320,16 @@ void SkeletonModel::setHandPosition(int jointIndex, const glm::vec3& position, c 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); + 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 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);