From 14ec1b62954073b8d218da2af34ed9d768c22149 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 29 Jan 2016 14:26:50 -0800 Subject: [PATCH] reduce hand IK coupling to hip position --- .../animation/src/AnimInverseKinematics.cpp | 29 +++++++++++++++---- libraries/animation/src/RotationConstraint.h | 3 ++ .../animation/src/SwingTwistConstraint.cpp | 6 ++-- .../animation/src/SwingTwistConstraint.h | 24 ++++++++------- 4 files changed, 44 insertions(+), 18 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index d46cb5b60a..b62eaca7fd 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -163,7 +163,6 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector 0) { _relativePoses[i].rot = _accumulators[i].getAverage(); @@ -261,16 +260,32 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe targetType == IKTarget::Type::HipsRelativeRotationAndPosition) { // compute the swing that would get get tip closer glm::vec3 targetLine = target.getTranslation() - jointPosition; + + const float MIN_AXIS_LENGTH = 1.0e-4f; + RotationConstraint* constraint = getConstraint(pivotIndex); + if (constraint && constraint->isLowerSpine()) { + // for these types of targets we only allow twist at the lower-spine + // (this prevents the hand targets from bending the spine too much and thereby driving the hips too far) + glm::vec3 twistAxis = absolutePoses[pivotIndex].trans - absolutePoses[pivotsParentIndex].trans; + float twistAxisLength = glm::length(twistAxis); + if (twistAxisLength > MIN_AXIS_LENGTH) { + // project leverArm and targetLine to the plane + twistAxis /= twistAxisLength; + leverArm -= glm::dot(leverArm, twistAxis) * twistAxis; + targetLine -= glm::dot(targetLine, twistAxis) * twistAxis; + } else { + leverArm = Vectors::ZERO; + targetLine = Vectors::ZERO; + } + } + glm::vec3 axis = glm::cross(leverArm, targetLine); float axisLength = glm::length(axis); - const float MIN_AXIS_LENGTH = 1.0e-4f; if (axisLength > MIN_AXIS_LENGTH) { - // compute deltaRotation for alignment (swings tip closer to target) + // compute angle of rotation that brings tip closer to target axis /= axisLength; float angle = acosf(glm::dot(leverArm, targetLine) / (glm::length(leverArm) * glm::length(targetLine))); - // NOTE: even when axisLength is not zero (e.g. lever-arm and pivot-arm are not quite aligned) it is - // still possible for the angle to be zero so we also check that to avoid unnecessary calculations. const float MIN_ADJUSTMENT_ANGLE = 1.0e-4f; if (angle > MIN_ADJUSTMENT_ANGLE) { // reduce angle by a fraction (for stability) @@ -663,6 +678,10 @@ void AnimInverseKinematics::initConstraints() { const float MAX_SPINE_SWING = PI / 14.0f; minDots.push_back(cosf(MAX_SPINE_SWING)); stConstraint->setSwingLimits(minDots); + if (0 == baseName.compare("Spine1", Qt::CaseInsensitive) + || 0 == baseName.compare("Spine", Qt::CaseInsensitive)) { + stConstraint->setLowerSpine(true); + } constraint = static_cast(stConstraint); } else if (baseName.startsWith("Hips2", Qt::CaseInsensitive)) { diff --git a/libraries/animation/src/RotationConstraint.h b/libraries/animation/src/RotationConstraint.h index 6926302ec8..0745500582 100644 --- a/libraries/animation/src/RotationConstraint.h +++ b/libraries/animation/src/RotationConstraint.h @@ -28,6 +28,9 @@ public: /// \return true if rotation is clamped virtual bool apply(glm::quat& rotation) const = 0; + /// \return true if this constraint is part of lower spine + virtual bool isLowerSpine() const { return false; } + protected: glm::quat _referenceRotation = glm::quat(); }; diff --git a/libraries/animation/src/SwingTwistConstraint.cpp b/libraries/animation/src/SwingTwistConstraint.cpp index c29f75202c..7386fb2bcd 100644 --- a/libraries/animation/src/SwingTwistConstraint.cpp +++ b/libraries/animation/src/SwingTwistConstraint.cpp @@ -123,7 +123,7 @@ void SwingTwistConstraint::setSwingLimits(const std::vector& swungDir // sort limits by theta std::sort(limits.begin(), limits.end()); - + // extrapolate evenly distributed limits for fast lookup table float deltaTheta = TWO_PI / (float)(numLimits); uint32_t rightIndex = 0; @@ -219,7 +219,7 @@ bool SwingTwistConstraint::apply(glm::quat& rotation) const { } 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; @@ -232,7 +232,7 @@ bool SwingTwistConstraint::apply(glm::quat& rotation) const { float theta = atan2f(-swingAxis.z, swingAxis.x); float minDot = _swingLimitFunction.getMinDot(theta); if (glm::dot(swungY, yAxis) < minDot) { - // The swing limits are violated so we extract the angle from midDot and + // The swing limits are violated so we extract the angle from midDot and // use it to supply a new rotation. swingAxis /= axisLength; swingRotation = glm::angleAxis(acosf(minDot), swingAxis); diff --git a/libraries/animation/src/SwingTwistConstraint.h b/libraries/animation/src/SwingTwistConstraint.h index 155f72a518..f36dc851ea 100644 --- a/libraries/animation/src/SwingTwistConstraint.h +++ b/libraries/animation/src/SwingTwistConstraint.h @@ -18,20 +18,20 @@ class SwingTwistConstraint : public RotationConstraint { public: - // The SwingTwistConstraint starts in the "referenceRotation" and then measures an initial twist + // The SwingTwistConstraint starts in the "referenceRotation" and then measures an initial twist // about the yAxis followed by a swing about some axis that lies in the XZ plane, such that the twist - // and swing combine to produce the rotation. Each partial rotation is constrained within limits + // and swing combine to produce the rotation. Each partial rotation is constrained within limits // then used to construct the new final rotation. SwingTwistConstraint(); /// \param minDots vector of minimum dot products between the twist and swung axes - /// \brief The values are minimum dot-products between the twist axis and the swung axes + /// \brief The values are minimum dot-products between the twist axis and the swung axes /// that correspond to swing axes equally spaced around the XZ plane. Another way to - /// think about it is that the dot-products correspond to correspond to angles (theta) - /// about the twist axis ranging from 0 to 2PI-deltaTheta (Note: the cyclic boundary + /// think about it is that the dot-products correspond to correspond to angles (theta) + /// about the twist axis ranging from 0 to 2PI-deltaTheta (Note: the cyclic boundary /// conditions are handled internally, so don't duplicate the dot-product at 2PI). - /// See the paper by Quang Liu and Edmond C. Prakash mentioned below for a more detailed + /// See the paper by Quang Liu and Edmond C. Prakash mentioned below for a more detailed /// description of how this works. void setSwingLimits(std::vector minDots); @@ -50,21 +50,24 @@ public: /// \return true if rotation is changed virtual bool apply(glm::quat& rotation) const override; + void setLowerSpine(bool lowerSpine) { _lowerSpine = lowerSpine; } + virtual bool isLowerSpine() const { return _lowerSpine; } + // SwingLimitFunction is an implementation of the constraint check described in the paper: // "The Parameterization of Joint Rotation with the Unit Quaternion" by Quang Liu and Edmond C. Prakash class SwingLimitFunction { public: SwingLimitFunction(); - + /// \brief use a uniform conical swing limit void setCone(float maxAngle); - + /// \brief use a vector of lookup values for swing limits void setMinDots(const std::vector& minDots); - + /// \return minimum dotProduct between reference and swung axes float getMinDot(float theta) const; - + protected: // the limits are stored in a lookup table with cyclic boundary conditions std::vector _minDots; @@ -84,6 +87,7 @@ protected: // We want to remember the LAST clamped boundary, so we an use it even when the far boundary is closer. // This reduces "pops" when the input twist angle goes far beyond and wraps around toward the far boundary. mutable int _lastTwistBoundary; + bool _lowerSpine { false }; }; #endif // hifi_SwingTwistConstraint_h