From 54af6af651117bbc0c4703abda38b14ff56b505a Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Fri, 23 Jun 2017 10:38:21 -0700 Subject: [PATCH] Fix for pole vector stability and knee pole vector computation --- .../animation/src/AnimInverseKinematics.cpp | 45 +++++++++----- libraries/animation/src/Rig.cpp | 62 ++++++++++++------- libraries/animation/src/Rig.h | 2 +- 3 files changed, 71 insertions(+), 38 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 3ea8937eec..e2e0d72d66 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -530,22 +530,33 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const if (dLen > EPSILON) { glm::vec3 dUnit = d / dLen; glm::vec3 e = midPose.xformVector(target.getPoleReferenceVector()); - glm::vec3 p = target.getPoleVector(); glm::vec3 eProj = e - glm::dot(e, dUnit) * dUnit; - glm::vec3 pProj = p - glm::dot(p, dUnit) * dUnit; float eProjLen = glm::length(eProj); - float pProjLen = glm::length(pProj); - if (eProjLen > EPSILON && pProjLen > EPSILON) { + 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); + + if (eProjLen > EPSILON && pProjLen > EPSILON) { // as pProjLen become orthognal to d, reduce the amount of rotation. float magnitude = easeOutExpo(pProjLen); - float dot = glm::clamp(glm::dot(eProj / eProjLen, pProj / pProjLen), 0.0f, 1.0f); float theta = acosf(dot); glm::vec3 cross = glm::cross(eProj, pProj); - float crossLen = glm::length(cross); - if (crossLen > EPSILON) { - glm::vec3 axis = cross / crossLen; + const float MIN_ADJUSTMENT_ANGLE = 0.001745f; // 0.1 degree + if (theta > MIN_ADJUSTMENT_ANGLE) { + glm::vec3 axis = dUnit; + if (glm::dot(cross, dUnit) < 0) { + axis = -dUnit; + } poleRot = glm::angleAxis(magnitude * theta, axis); } } @@ -562,8 +573,17 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const glm::vec3 dUnit = d / dLen; glm::vec3 e = midPose.xformVector(target.getPoleReferenceVector()); - glm::vec3 p = target.getPoleVector(); 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; const float PROJ_VECTOR_LEN = 10.0f; @@ -573,11 +593,8 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const geomToWorldPose.xformPoint(topPose.trans()), YELLOW); DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint), - geomToWorldPose.xformPoint(midPoint + PROJ_VECTOR_LEN * glm::normalize(eProj)), + geomToWorldPose.xformPoint(midPoint + PROJ_VECTOR_LEN * glm::normalize(e)), RED); - DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint), - geomToWorldPose.xformPoint(midPoint + PROJ_VECTOR_LEN * glm::normalize(pProj)), - GREEN); DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint), geomToWorldPose.xformPoint(midPoint + POLE_VECTOR_LEN * glm::normalize(p)), BLUE); @@ -1060,7 +1077,7 @@ void AnimInverseKinematics::initConstraints() { // y | // | | // | O---O---O RightUpLeg - // z | | Hips2 | + // z | | Hips | // \ | | | // \| | | // x -----+ O O RightLeg diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp index 51480e8ef1..8466b79d1d 100644 --- a/libraries/animation/src/Rig.cpp +++ b/libraries/animation/src/Rig.cpp @@ -1265,14 +1265,14 @@ void Rig::updateFeet(bool leftFootEnabled, bool rightFootEnabled, const AnimPose int hipsIndex = indexOfJoint("Hips"); if (leftFootEnabled) { - glm::vec3 footPosition = leftFootPose.trans(); - glm::quat footRotation = leftFootPose.rot(); - _animVars.set("leftFootPosition", footPosition); - _animVars.set("leftFootRotation", footRotation); + _animVars.set("leftFootPosition", leftFootPose.trans()); + _animVars.set("leftFootRotation", leftFootPose.rot()); _animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition); int footJointIndex = _animSkeleton->nameToJointIndex("LeftFoot"); - glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, footRotation, hipsIndex); + int kneeJointIndex = _animSkeleton->nameToJointIndex("LeftLeg"); + int upLegJointIndex = _animSkeleton->nameToJointIndex("LeftUpLeg"); + glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, kneeJointIndex, upLegJointIndex, hipsIndex, leftFootPose); // smooth toward desired pole vector from previous pole vector... to reduce jitter if (!_prevLeftFootPoleVectorValid) { @@ -1295,14 +1295,14 @@ void Rig::updateFeet(bool leftFootEnabled, bool rightFootEnabled, const AnimPose } if (rightFootEnabled) { - glm::vec3 footPosition = rightFootPose.trans(); - glm::quat footRotation = rightFootPose.rot(); - _animVars.set("rightFootPosition", footPosition); - _animVars.set("rightFootRotation", footRotation); + _animVars.set("rightFootPosition", rightFootPose.trans()); + _animVars.set("rightFootRotation", rightFootPose.rot()); _animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition); int footJointIndex = _animSkeleton->nameToJointIndex("RightFoot"); - glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, footRotation, hipsIndex); + int kneeJointIndex = _animSkeleton->nameToJointIndex("RightLeg"); + int upLegJointIndex = _animSkeleton->nameToJointIndex("RightUpLeg"); + glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, kneeJointIndex, upLegJointIndex, hipsIndex, rightFootPose); // smooth toward desired pole vector from previous pole vector... to reduce jitter if (!_prevRightFootPoleVectorValid) { @@ -1375,20 +1375,23 @@ glm::vec3 Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIn AnimPose handPose = _externalPoseSet._absolutePoses[handIndex]; AnimPose elbowPose = _externalPoseSet._absolutePoses[elbowIndex]; AnimPose armPose = _externalPoseSet._absolutePoses[armIndex]; + + // ray from hand to arm. glm::vec3 d = glm::normalize(handPose.trans() - armPose.trans()); float sign = isLeft ? 1.0f : -1.0f; + // form a plane normal to the hips x-axis. glm::vec3 n = hipsPose.rot() * Vectors::UNIT_X; glm::vec3 y = hipsPose.rot() * Vectors::UNIT_Y; - // project d onto n. + + // project d onto this plane glm::vec3 dProj = d - glm::dot(d, n) * n; + // give dProj a bit of offset away from the body. float avatarScale = extractUniformScale(_modelOffset); const float LATERAL_OFFSET = 0.333f * avatarScale; const float VERTICAL_OFFSET = -0.333f * avatarScale; - - // give dProj a bit of offset away from the body. glm::vec3 dProjWithOffset = dProj + sign * LATERAL_OFFSET * n + y * VERTICAL_OFFSET; // rotate dProj by 90 degrees to get the poleVector. @@ -1402,19 +1405,32 @@ glm::vec3 Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIn return glm::normalize(poleAdjust * poleVector); } -glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, const glm::quat& footTargetOrientation, int hipsIndex) const { +glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, int kneeIndex, int upLegIndex, int hipsIndex, const AnimPose& targetFootPose) const { - AnimPose defaultPose = _animSkeleton->getAbsoluteDefaultPose(footJointIndex); - glm::vec3 localForward = glm::inverse(defaultPose.rot()) * Vectors::UNIT_Z; - glm::vec3 footForward = footTargetOrientation * localForward; + AnimPose hipsPose = _externalPoseSet._absolutePoses[hipsIndex]; + AnimPose footPose = targetFootPose; + AnimPose kneePose = _externalPoseSet._absolutePoses[kneeIndex]; + AnimPose upLegPose = _externalPoseSet._absolutePoses[upLegIndex]; - // compute the forward direction of the hips. - glm::quat hipsRotation = _externalPoseSet._absolutePoses[hipsIndex].rot(); - glm::vec3 hipsForward = hipsRotation * Vectors::UNIT_Z; + // ray from foot to upLeg + glm::vec3 d = glm::normalize(footPose.trans() - upLegPose.trans()); - // blend between the hips and the foot. - const float FOOT_TO_HIPS_BLEND_FACTOR = 0.5f; - return glm::normalize(lerp(footForward, hipsForward, FOOT_TO_HIPS_BLEND_FACTOR)); + // form a plane normal to the hips x-axis + glm::vec3 n = hipsPose.rot() * Vectors::UNIT_X; + glm::vec3 y = hipsPose.rot() * Vectors::UNIT_Y; + + // project d onto this plane + glm::vec3 dProj = d - glm::dot(d, n) * n; + + // rotate dProj by 90 degrees to get the poleVector. + glm::vec3 poleVector = glm::angleAxis(-PI / 2.0f, n) * dProj; + + // blend the foot oreintation into the pole vector + glm::quat kneeToFootDelta = footPose.rot() * glm::inverse(kneePose.rot()); + const float WRIST_POLE_ADJUST_FACTOR = 0.5f; + glm::quat poleAdjust = quatLerp(Quaternions::IDENTITY, kneeToFootDelta, WRIST_POLE_ADJUST_FACTOR); + + return glm::normalize(poleAdjust * poleVector); } void Rig::updateFromControllerParameters(const ControllerParameters& params, float dt) { diff --git a/libraries/animation/src/Rig.h b/libraries/animation/src/Rig.h index 413b598079..c17a7b9c8f 100644 --- a/libraries/animation/src/Rig.h +++ b/libraries/animation/src/Rig.h @@ -239,7 +239,7 @@ protected: void calcAnimAlpha(float speed, const std::vector& referenceSpeeds, float* alphaOut) const; glm::vec3 calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, int hipsIndex, bool isLeft) const; - glm::vec3 calculateKneePoleVector(int footJointIndex, const glm::quat& footTargetOrientation, int hipsIndex) const; + glm::vec3 calculateKneePoleVector(int footJointIndex, int kneeJoint, int upLegIndex, int hipsIndex, const AnimPose& targetFootPose) const; AnimPose _modelOffset; // model to rig space AnimPose _geometryOffset; // geometry to model space (includes unit offset & fst offsets)