From 467a7eaf2dbc47245daf49299a014f8d5a68fda0 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 11 Jan 2018 15:40:53 -0800 Subject: [PATCH 1/3] Fix for elbow glitches The pole vector constraint calculation within the IK system would sometimes compute the incorrect rotations. This would be visible as an instantaneous snap of the elbow joint as the bicep was curled. When applying pole vector constraints, there needs to be two methods of determining the current orientation of the elbow joint. One for when the arm/elbow joint is bent, and one for when the arm/elbow is straight. Previously, the way we would switch between these two solutions could cause a large rotation delta to accur between very small angles. Now we use the more accurate method (1) more often, and we smoothly blend between the solutions as the joint gets straighter. --- .../animation/src/AnimInverseKinematics.cpp | 60 ++++++++++++++----- 1 file changed, 44 insertions(+), 16 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index d17fbebf3a..15c7cfbb57 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -591,19 +591,31 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const glm::vec3 d = basePose.trans() - topPose.trans(); float dLen = glm::length(d); if (dLen > EPSILON) { + glm::vec3 dUnit = d / dLen; glm::vec3 e = midPose.xformVector(target.getPoleReferenceVector()); + + // if mid joint is straight use the reference vector to compute eProj, otherwise use reference vector. + // however if mid joint angle is in between the two blend between both solutions. + vec3 u = normalize(basePose.trans() - midPose.trans()); + vec3 v = normalize(topPose.trans() - midPose.trans()); + + const float LERP_THRESHOLD = 3.05433f; // 175 deg + const float BENT_THRESHOLD = 2.96706f; // 170 deg + + float jointAngle = acos(dot(u, v)); + if (jointAngle < BENT_THRESHOLD) { + glm::vec3 midPoint = topPose.trans() + d * 0.5f; + e = normalize(midPose.trans() - midPoint); + } else if (jointAngle < LERP_THRESHOLD) { + glm::vec3 midPoint = topPose.trans() + d * 0.5f; + float alpha = (jointAngle - LERP_THRESHOLD) / (BENT_THRESHOLD - LERP_THRESHOLD); + e = lerp(e, normalize(midPose.trans() - midPoint), alpha); + } + glm::vec3 eProj = e - glm::dot(e, dUnit) * dUnit; float eProjLen = glm::length(eProj); - const float MIN_EPROJ_LEN = 0.5f; - if (eProjLen < MIN_EPROJ_LEN) { - glm::vec3 midPoint = topPose.trans() + d * 0.5f; - e = midPose.trans() - midPoint; - eProj = e - glm::dot(e, dUnit) * dUnit; - eProjLen = glm::length(eProj); - } - glm::vec3 p = target.getPoleVector(); glm::vec3 pProj = p - glm::dot(p, dUnit) * dUnit; float pProjLen = glm::length(pProj); @@ -636,15 +648,31 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const glm::vec3 dUnit = d / dLen; glm::vec3 e = midPose.xformVector(target.getPoleReferenceVector()); + + + // if mid joint is straight use the reference vector to compute eProj, otherwise use reference vector. + // however if mid joint angle is in between the two blend between both solutions. + vec3 u = normalize(basePose.trans() - midPose.trans()); + vec3 v = normalize(topPose.trans() - midPose.trans()); + + const float LERP_THRESHOLD = 3.05433f; // 175 deg + const float BENT_THRESHOLD = 2.96706f; // 170 deg + + float jointAngle = acos(dot(u, v)); + glm::vec4 eColor = RED; + if (jointAngle < BENT_THRESHOLD) { + glm::vec3 midPoint = topPose.trans() + d * 0.5f; + e = normalize(midPose.trans() - midPoint); + eColor = GREEN; + } else if (jointAngle < LERP_THRESHOLD) { + glm::vec3 midPoint = topPose.trans() + d * 0.5f; + float alpha = (jointAngle - LERP_THRESHOLD) / (BENT_THRESHOLD - LERP_THRESHOLD); + e = lerp(e, normalize(midPose.trans() - midPoint), alpha); + eColor = YELLOW; + } + glm::vec3 eProj = e - glm::dot(e, dUnit) * dUnit; float eProjLen = glm::length(eProj); - const float MIN_EPROJ_LEN = 0.5f; - if (eProjLen < MIN_EPROJ_LEN) { - glm::vec3 midPoint = topPose.trans() + d * 0.5f; - e = midPose.trans() - midPoint; - eProj = e - glm::dot(e, dUnit) * dUnit; - eProjLen = glm::length(eProj); - } glm::vec3 p = target.getPoleVector(); const float PROJ_VECTOR_LEN = 10.0f; @@ -655,7 +683,7 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const YELLOW); DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint), geomToWorldPose.xformPoint(midPoint + PROJ_VECTOR_LEN * glm::normalize(e)), - RED); + eColor); DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint), geomToWorldPose.xformPoint(midPoint + POLE_VECTOR_LEN * glm::normalize(p)), BLUE); From 983e1fe0759e914de44f1d18f99f658b04afb249 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Thu, 11 Jan 2018 17:57:58 -0800 Subject: [PATCH 2/3] warning fixes: removed unused variable. --- libraries/animation/src/AnimInverseKinematics.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 15c7cfbb57..7bf250a32c 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -649,7 +649,6 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const glm::vec3 dUnit = d / dLen; glm::vec3 e = midPose.xformVector(target.getPoleReferenceVector()); - // if mid joint is straight use the reference vector to compute eProj, otherwise use reference vector. // however if mid joint angle is in between the two blend between both solutions. vec3 u = normalize(basePose.trans() - midPose.trans()); @@ -671,9 +670,6 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const eColor = YELLOW; } - glm::vec3 eProj = e - glm::dot(e, dUnit) * dUnit; - float eProjLen = glm::length(eProj); - glm::vec3 p = target.getPoleVector(); const float PROJ_VECTOR_LEN = 10.0f; const float POLE_VECTOR_LEN = 100.0f; From bfd18e3696678e71f97dedd62ca4724ac9baa351 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 12 Jan 2018 09:07:50 -0800 Subject: [PATCH 3/3] unused variable warning fix --- libraries/animation/src/AnimInverseKinematics.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 7bf250a32c..c8388c3b12 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -646,7 +646,6 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const AnimPose geomToWorldPose = AnimPose(context.getRigToWorldMatrix() * context.getGeometryToRigMatrix()); - glm::vec3 dUnit = d / dLen; glm::vec3 e = midPose.xformVector(target.getPoleReferenceVector()); // if mid joint is straight use the reference vector to compute eProj, otherwise use reference vector.