From c06d76f7f06fe05213f138fd3532d7bdba89101f Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 4 Mar 2016 10:56:29 -0800 Subject: [PATCH] remove twist constraints on hands also tighten swing limit on shoulder --- .../animation/src/AnimInverseKinematics.cpp | 8 +- .../animation/src/SwingTwistConstraint.cpp | 79 ++++++++++--------- 2 files changed, 47 insertions(+), 40 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 48ad9b852d..6a29ad61ac 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -627,6 +627,9 @@ void AnimInverseKinematics::initConstraints() { } else if (0 == baseName.compare("Hand", Qt::CaseSensitive)) { SwingTwistConstraint* stConstraint = new SwingTwistConstraint(); stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot); + stConstraint->setTwistLimits(0.0f, 0.0f); // max == min, disables twist limits + + /* KEEP THIS CODE for future experimentation -- twist limits for hands const float MAX_HAND_TWIST = 3.0f * PI / 5.0f; const float MIN_HAND_TWIST = -PI / 2.0f; if (isLeft) { @@ -634,8 +637,9 @@ void AnimInverseKinematics::initConstraints() { } else { stConstraint->setTwistLimits(MIN_HAND_TWIST, MAX_HAND_TWIST); } + */ - /* KEEP THIS CODE for future experimentation + /* KEEP THIS CODE for future experimentation -- non-symmetrical swing limits for wrist * a more complicated wrist with asymmetric cone // these directions are approximate swing limits in parent-frame // NOTE: they don't need to be normalized @@ -670,7 +674,7 @@ void AnimInverseKinematics::initConstraints() { stConstraint->setTwistLimits(-MAX_SHOULDER_TWIST, MAX_SHOULDER_TWIST); std::vector minDots; - const float MAX_SHOULDER_SWING = PI / 6.0f; + const float MAX_SHOULDER_SWING = PI / 20.0f; minDots.push_back(cosf(MAX_SHOULDER_SWING)); stConstraint->setSwingLimits(minDots); diff --git a/libraries/animation/src/SwingTwistConstraint.cpp b/libraries/animation/src/SwingTwistConstraint.cpp index 7386fb2bcd..3bf661612e 100644 --- a/libraries/animation/src/SwingTwistConstraint.cpp +++ b/libraries/animation/src/SwingTwistConstraint.cpp @@ -182,49 +182,52 @@ bool SwingTwistConstraint::apply(glm::quat& rotation) const { glm::vec3 twistedX = twistRotation * xAxis; twistAngle *= copysignf(1.0f, glm::dot(glm::cross(xAxis, twistedX), yAxis)); - // adjust measured twistAngle according to clamping history - switch (_lastTwistBoundary) { - case LAST_CLAMP_LOW_BOUNDARY: - // clamp to min - if (twistAngle > _maxTwist) { - twistAngle -= TWO_PI; - } - break; - case LAST_CLAMP_HIGH_BOUNDARY: - // clamp to max - if (twistAngle < _minTwist) { - twistAngle += TWO_PI; - } - break; - default: // LAST_CLAMP_NO_BOUNDARY - // clamp to nearest boundary - float midBoundary = 0.5f * (_maxTwist + _minTwist + TWO_PI); - if (twistAngle > midBoundary) { - // lower boundary is closer --> phase down one cycle - twistAngle -= TWO_PI; - } else if (twistAngle < midBoundary - TWO_PI) { - // higher boundary is closer --> phase up one cycle - twistAngle += TWO_PI; - } - break; - } + bool somethingClamped = false; + if (_minTwist != _maxTwist) { + // adjust measured twistAngle according to clamping history + switch (_lastTwistBoundary) { + case LAST_CLAMP_LOW_BOUNDARY: + // clamp to min + if (twistAngle > _maxTwist) { + twistAngle -= TWO_PI; + } + break; + case LAST_CLAMP_HIGH_BOUNDARY: + // clamp to max + if (twistAngle < _minTwist) { + twistAngle += TWO_PI; + } + break; + default: // LAST_CLAMP_NO_BOUNDARY + // clamp to nearest boundary + float midBoundary = 0.5f * (_maxTwist + _minTwist + TWO_PI); + if (twistAngle > midBoundary) { + // lower boundary is closer --> phase down one cycle + twistAngle -= TWO_PI; + } else if (twistAngle < midBoundary - TWO_PI) { + // higher boundary is closer --> phase up one cycle + twistAngle += TWO_PI; + } + break; + } - // clamp twistAngle - float clampedTwistAngle = glm::clamp(twistAngle, _minTwist, _maxTwist); - bool twistWasClamped = (twistAngle != clampedTwistAngle); + // clamp twistAngle + float clampedTwistAngle = glm::clamp(twistAngle, _minTwist, _maxTwist); + somethingClamped = (twistAngle != clampedTwistAngle); - // remember twist's clamp boundary history - if (twistWasClamped) { - _lastTwistBoundary = (twistAngle > clampedTwistAngle) ? LAST_CLAMP_HIGH_BOUNDARY : LAST_CLAMP_LOW_BOUNDARY; - } else { - _lastTwistBoundary = LAST_CLAMP_NO_BOUNDARY; + // remember twist's clamp boundary history + if (somethingClamped) { + _lastTwistBoundary = (twistAngle > clampedTwistAngle) ? LAST_CLAMP_HIGH_BOUNDARY : LAST_CLAMP_LOW_BOUNDARY; + twistAngle = clampedTwistAngle; + } else { + _lastTwistBoundary = LAST_CLAMP_NO_BOUNDARY; + } } // clamp the swing // The swingAxis is always perpendicular to the reference axis (yAxis in the constraint's frame). glm::vec3 swungY = swingRotation * yAxis; glm::vec3 swingAxis = glm::cross(yAxis, swungY); - bool swingWasClamped = false; float axisLength = glm::length(swingAxis); if (axisLength > EPSILON) { // The limit of swing is a function of "theta" which can be computed from the swingAxis @@ -236,13 +239,13 @@ bool SwingTwistConstraint::apply(glm::quat& rotation) const { // use it to supply a new rotation. swingAxis /= axisLength; swingRotation = glm::angleAxis(acosf(minDot), swingAxis); - swingWasClamped = true; + somethingClamped = true; } } - if (swingWasClamped || twistWasClamped) { + if (somethingClamped) { // update the rotation - twistRotation = glm::angleAxis(clampedTwistAngle, yAxis); + twistRotation = glm::angleAxis(twistAngle, yAxis); rotation = swingRotation * twistRotation * _referenceRotation; return true; }