diff --git a/libraries/animation/src/SwingTwistConstraint.cpp b/libraries/animation/src/SwingTwistConstraint.cpp index 3a2606c5ce..dd0a9ce0e8 100644 --- a/libraries/animation/src/SwingTwistConstraint.cpp +++ b/libraries/animation/src/SwingTwistConstraint.cpp @@ -26,23 +26,150 @@ const int LAST_CLAMP_HIGH_BOUNDARY = 1; SwingTwistConstraint::SwingLimitFunction::SwingLimitFunction() { _minDots.push_back(-1.0f); _minDots.push_back(-1.0f); + + _minDotIndexA = -1; + _minDotIndexB = -1; } +// In order to support the dynamic adjustment to swing limits we require +// that minDots have a minimum number of elements: +const int MIN_NUM_DOTS = 8; + void SwingTwistConstraint::SwingLimitFunction::setMinDots(const std::vector& minDots) { - uint32_t numDots = (uint32_t)minDots.size(); + int numDots = (int)minDots.size(); _minDots.clear(); if (numDots == 0) { - // push two copies of MIN_MINDOT - _minDots.push_back(MIN_MINDOT); + // push multiple copies of MIN_MINDOT + for (int i = 0; i < MIN_NUM_DOTS; ++i) { + _minDots.push_back(MIN_MINDOT); + } + // push one more for cyclic boundary conditions _minDots.push_back(MIN_MINDOT); } else { - _minDots.reserve(numDots); - for (uint32_t i = 0; i < numDots; ++i) { - _minDots.push_back(glm::clamp(minDots[i], MIN_MINDOT, MAX_MINDOT)); + // for minimal fidelity in the dynamic adjustment we expand the swing limit data until + // we have enough data points + int trueNumDots = numDots; + int numFiller = 0; + while(trueNumDots < MIN_NUM_DOTS) { + numFiller++; + trueNumDots += numDots; } - // push the first value to the back to establish cyclic boundary conditions + _minDots.reserve(trueNumDots); + + for (int i = 0; i < numDots; ++i) { + // push the next value + _minDots.push_back(glm::clamp(minDots[i], MIN_MINDOT, MAX_MINDOT)); + + if (numFiller > 0) { + // compute endpoints of line segment + float nearDot = glm::clamp(minDots[i], MIN_MINDOT, MAX_MINDOT); + int k = (i + 1) % numDots; + float farDot = glm::clamp(minDots[k], MIN_MINDOT, MAX_MINDOT); + + // fill the gap with interpolated values + for (int j = 0; j < numFiller; ++j) { + float delta = (float)(j + 1) / float(numFiller + 1); + _minDots.push_back((1.0f - delta) * nearDot + delta * farDot); + } + } + } + // push the first value to the back to for cyclic boundary conditions _minDots.push_back(_minDots[0]); } + _minDotIndexA = -1; + _minDotIndexB = -1; +} + +/// \param angle radian angle to update +/// \param minDotAdjustment minimum dot limit at that angle +void SwingTwistConstraint::SwingLimitFunction::dynamicallyAdjustMinDots(float theta, float minDotAdjustment) { + // What does "dynamic adjustment" mean? + // + // Consider a limitFunction that looks like this: + // + // 1+ + // | valid space + // | + // +-----+-----+-----+-----+-----+-----+-----+-----+ + // | + // | invalid space + // 0+------------------------------------------------ + // 0 pi/2 pi 3pi/2 2pi + // theta ---> + // + // If we wanted to modify the envelope to accept a single invalid point X + // then we would need to modify neighboring values A and B accordingly: + // + // 1+ adjustment for X at some thetaX + // | | + // | | + // +-----+. V .+-----+-----+-----+-----+ + // | - - + // | ' A--X--B ' + // 0+------------------------------------------------ + // 0 pi/2 pi 3pi/2 2pi + // + // The code below computes the values of A and B such that the line between them + // passes through the point X, and we get reasonable interpolation for nearby values + // of theta. The old AB values are saved for later restore. + + if (_minDotIndexA > -1) { + // retstore old values + _minDots[_minDotIndexA] = _minDotA; + _minDots[_minDotIndexB] = _minDotB; + + // handle cyclic boundary conditions + int lastIndex = (int)_minDots.size() - 1; + if (_minDotIndexA == 0) { + _minDots[lastIndex] = _minDotA; + } else if (_minDotIndexB == lastIndex) { + _minDots[0] = _minDotB; + } + } + + // extract the positive normalized fractional part of the theta + float integerPart; + float normalizedAngle = modff(theta / TWO_PI, &integerPart); + if (normalizedAngle < 0.0f) { + normalizedAngle += 1.0f; + } + + // interpolate between the two nearest points in the curve + float delta = modff(normalizedAngle * (float)(_minDots.size() - 1), &integerPart); + int indexA = (int)(integerPart); + int indexB = (indexA + 1) % _minDots.size(); + float interpolatedDot = _minDots[indexA] * (1.0f - delta) + _minDots[indexB] * delta; + + if (minDotAdjustment < interpolatedDot) { + // minDotAdjustment is outside the existing bounds so we must modify + + // remember the indices + _minDotIndexA = indexA; + _minDotIndexB = indexB; + + // save the old minDots + _minDotA = _minDots[_minDotIndexA]; + _minDotB = _minDots[_minDotIndexB]; + + // compute replacement values to _minDots that will provide a line segment + // that passes through minDotAdjustment while balancing the distortion between A and B. + // Note: the derivation of these formulae is left as an exercise to the reader. + float twiceUndershoot = 2.0f * (minDotAdjustment - interpolatedDot); + _minDots[_minDotIndexA] -= twiceUndershoot * (delta + 0.5f) * (delta - 1.0f); + _minDots[_minDotIndexB] -= twiceUndershoot * delta * (delta - 1.5f); + + // handle cyclic boundary conditions + int lastIndex = (int)_minDots.size() - 1; + if (_minDotIndexA == 0) { + _minDots[lastIndex] = _minDots[_minDotIndexA]; + } else if (_minDotIndexB == lastIndex) { + _minDots[0] = _minDots[_minDotIndexB]; + } + } else { + // minDotAdjustment is inside bounds so there is nothing to do + _minDotIndexA = -1; + _minDotIndexB = -1; + } } float SwingTwistConstraint::SwingLimitFunction::getMinDot(float theta) const { @@ -83,12 +210,12 @@ void SwingTwistConstraint::setSwingLimits(const std::vector& swungDir }; std::vector limits; - uint32_t numLimits = (uint32_t)swungDirections.size(); + int numLimits = (int)swungDirections.size(); limits.reserve(numLimits); // compute the limit pairs: const glm::vec3 yAxis = glm::vec3(0.0f, 1.0f, 0.0f); - for (uint32_t i = 0; i < numLimits; ++i) { + for (int i = 0; i < numLimits; ++i) { float directionLength = glm::length(swungDirections[i]); if (directionLength > EPSILON) { glm::vec3 swingAxis = glm::cross(yAxis, swungDirections[i]); @@ -101,7 +228,7 @@ void SwingTwistConstraint::setSwingLimits(const std::vector& swungDir } std::vector minDots; - numLimits = (uint32_t)limits.size(); + numLimits = (int)limits.size(); if (numLimits == 0) { // trivial case: nearly free constraint std::vector minDots; @@ -119,10 +246,10 @@ void SwingTwistConstraint::setSwingLimits(const std::vector& swungDir // extrapolate evenly distributed limits for fast lookup table float deltaTheta = TWO_PI / (float)(numLimits); - uint32_t rightIndex = 0; - for (uint32_t i = 0; i < numLimits; ++i) { + int rightIndex = 0; + for (int i = 0; i < numLimits; ++i) { float theta = (float)i * deltaTheta; - uint32_t leftIndex = (rightIndex - 1) % numLimits; + int leftIndex = (rightIndex - 1) % numLimits; while (rightIndex < numLimits && theta > limits[rightIndex]._theta) { leftIndex = rightIndex++; } @@ -245,6 +372,20 @@ bool SwingTwistConstraint::apply(glm::quat& rotation) const { return false; } +void SwingTwistConstraint::dynamicallyAdjustLimits(const glm::quat& rotation) { + glm::quat postRotation = rotation * glm::inverse(_referenceRotation); + glm::quat swingRotation, twistRotation; + + const glm::vec3 yAxis(0.0f, 1.0f, 0.0f); + swingTwistDecomposition(postRotation, yAxis, swingRotation, twistRotation); + + // we currently only handle swing limits + glm::vec3 swungY = swingRotation * yAxis; + glm::vec3 swingAxis = glm::cross(yAxis, swungY); + float theta = atan2f(-swingAxis.z, swingAxis.x); + _swingLimitFunction.dynamicallyAdjustMinDots(theta, swungY.y); +} + void SwingTwistConstraint::clearHistory() { _lastTwistBoundary = LAST_CLAMP_NO_BOUNDARY; } diff --git a/libraries/animation/src/SwingTwistConstraint.h b/libraries/animation/src/SwingTwistConstraint.h index 620e63e98b..df9da8cabe 100644 --- a/libraries/animation/src/SwingTwistConstraint.h +++ b/libraries/animation/src/SwingTwistConstraint.h @@ -53,8 +53,18 @@ public: void setLowerSpine(bool lowerSpine) { _lowerSpine = lowerSpine; } virtual bool isLowerSpine() const override { return _lowerSpine; } + /// \param rotation rotation to allow + /// \brief clear previous adjustment and adjust constraint limits to allow rotation + void dynamicallyAdjustLimits(const glm::quat& rotation); + + // for testing purposes + const std::vector& getMinDots() { return _swingLimitFunction.getMinDots(); } + // 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 + // + // The "dynamic adjustment" feature allows us to change the limits on the fly for one particular theta angle. + // class SwingLimitFunction { public: SwingLimitFunction(); @@ -62,12 +72,26 @@ public: /// \brief use a vector of lookup values for swing limits void setMinDots(const std::vector& minDots); + /// \param theta radian angle to new minDot + /// \param minDot minimum dot limit + /// \brief updates swing constraint to permit minDot at theta + void dynamicallyAdjustMinDots(float theta, float minDot); + /// \return minimum dotProduct between reference and swung axes float getMinDot(float theta) const; - protected: + // for testing purposes + const std::vector& getMinDots() { return _minDots; } + + private: // the limits are stored in a lookup table with cyclic boundary conditions std::vector _minDots; + + // these values used to restore dynamic adjustment + float _minDotA; + float _minDotB; + int8_t _minDotIndexA; + int8_t _minDotIndexB; }; /// \return reference to SwingLimitFunction instance for unit-testing diff --git a/tests/animation/src/RotationConstraintTests.cpp b/tests/animation/src/RotationConstraintTests.cpp index 7aacf26826..b0351721ae 100644 --- a/tests/animation/src/RotationConstraintTests.cpp +++ b/tests/animation/src/RotationConstraintTests.cpp @@ -56,7 +56,7 @@ void RotationConstraintTests::testElbowConstraint() { float startAngle = minAngle + smallAngle; float endAngle = maxAngle - smallAngle; float deltaAngle = (endAngle - startAngle) / (float)(numChecks - 1); - + for (float angle = startAngle; angle < endAngle + 0.5f * deltaAngle; angle += deltaAngle) { glm::quat inputRotation = glm::angleAxis(angle, hingeAxis) * referenceRotation; glm::quat outputRotation = inputRotation; @@ -115,9 +115,9 @@ void RotationConstraintTests::testSwingTwistConstraint() { shoulder.setTwistLimits(minTwistAngle, maxTwistAngle); float lowDot = 0.25f; float highDot = 0.75f; - // The swing constriants are more interesting: a vector of minimum dot products + // The swing constriants are more interesting: a vector of minimum dot products // as a function of theta around the twist axis. Our test function will be shaped - // like the square wave with amplitudes 0.25 and 0.75: + // like a square wave with amplitudes 0.25 and 0.75: // // | // 0.75 - o---o---o---o @@ -308,3 +308,250 @@ void RotationConstraintTests::testSwingTwistConstraint() { } } +void RotationConstraintTests::testDynamicSwingLimitFunction() { + SwingTwistConstraint::SwingLimitFunction limitFunction; + const float ACCEPTABLE_ERROR = 1.0e-6f; + + const float adjustmentDot = -0.5f; + + const float MIN_DOT = 0.5f; + { // initialize limitFunction + std::vector minDots; + minDots.push_back(MIN_DOT); + limitFunction.setMinDots(minDots); + } + + std::vector referenceDots; + { // verify limits and initialize referenceDots + const int MIN_NUM_DOTS = 8; + const std::vector& minDots = limitFunction.getMinDots(); + QVERIFY(minDots.size() >= MIN_NUM_DOTS); + + int numDots = (int)minDots.size(); + for (int i = 0; i < numDots; ++i) { + QCOMPARE_WITH_RELATIVE_ERROR(minDots[i], MIN_DOT, ACCEPTABLE_ERROR); + referenceDots.push_back(minDots[i]); + } + } + { // dynamically adjust limits + const std::vector& minDots = limitFunction.getMinDots(); + int numDots = (int)minDots.size(); + + float deltaTheta = TWO_PI / (float)(numDots - 1); + int indexA = 2; + int indexB = (indexA + 1) % numDots; + + { // dynamically adjust a data point + float theta = deltaTheta * (float)indexA; + float interpolatedDot = limitFunction.getMinDot(theta); + + // change indexA + limitFunction.dynamicallyAdjustMinDots(theta, adjustmentDot); + QCOMPARE_WITH_ABS_ERROR(limitFunction.getMinDot(theta), adjustmentDot, ACCEPTABLE_ERROR); // adjustmentDot at theta + QCOMPARE_WITH_ABS_ERROR(minDots[indexA], adjustmentDot, ACCEPTABLE_ERROR); // indexA has changed + QCOMPARE_WITH_ABS_ERROR(minDots[indexB], referenceDots[indexB], ACCEPTABLE_ERROR); // indexB has not changed + + // change indexB + theta = deltaTheta * (float)indexB; + limitFunction.dynamicallyAdjustMinDots(theta, adjustmentDot); + QCOMPARE_WITH_ABS_ERROR(limitFunction.getMinDot(theta), adjustmentDot, ACCEPTABLE_ERROR); // adjustmentDot at theta + QCOMPARE_WITH_ABS_ERROR(minDots[indexA], referenceDots[indexA], ACCEPTABLE_ERROR); // indexA has been restored + QCOMPARE_WITH_ABS_ERROR(minDots[indexB], adjustmentDot, ACCEPTABLE_ERROR); // indexB has changed + + // restore + limitFunction.dynamicallyAdjustMinDots(theta, referenceDots[indexB] + 0.01f); // restore with a larger dot + QCOMPARE_WITH_ABS_ERROR(limitFunction.getMinDot(theta), interpolatedDot, ACCEPTABLE_ERROR); // restored + QCOMPARE_WITH_ABS_ERROR(minDots[indexA], referenceDots[indexA], ACCEPTABLE_ERROR); // indexA is restored + QCOMPARE_WITH_ABS_ERROR(minDots[indexB], referenceDots[indexB], ACCEPTABLE_ERROR); // indexB is restored + } + { // dynamically adjust halfway between data points + float theta = deltaTheta * 0.5f * (float)(indexA + indexB); // halfway between two points + float interpolatedDot = limitFunction.getMinDot(theta); + float deltaDot = adjustmentDot - interpolatedDot; + limitFunction.dynamicallyAdjustMinDots(theta, adjustmentDot); + QCOMPARE_WITH_ABS_ERROR(limitFunction.getMinDot(theta), adjustmentDot, ACCEPTABLE_ERROR); // adjustmentDot at theta + QCOMPARE_WITH_ABS_ERROR(minDots[indexA], referenceDots[indexA] + deltaDot, ACCEPTABLE_ERROR); // indexA has changed + QCOMPARE_WITH_ABS_ERROR(minDots[indexB], referenceDots[indexB] + deltaDot, ACCEPTABLE_ERROR); // indexB has changed + + limitFunction.dynamicallyAdjustMinDots(theta, interpolatedDot + 0.01f); // reset with something larger + QCOMPARE_WITH_ABS_ERROR(limitFunction.getMinDot(theta), interpolatedDot, ACCEPTABLE_ERROR); // restored + QCOMPARE_WITH_ABS_ERROR(minDots[indexA], referenceDots[indexA], ACCEPTABLE_ERROR); // indexA is restored + QCOMPARE_WITH_ABS_ERROR(minDots[indexB], referenceDots[indexB], ACCEPTABLE_ERROR); // indexB is restored + } + { // dynamically adjust one-quarter between data points + float theta = deltaTheta * ((float)indexA + 0.25f); // one quarter past A towards B + float interpolatedDot = limitFunction.getMinDot(theta); + limitFunction.dynamicallyAdjustMinDots(theta, adjustmentDot); + QCOMPARE_WITH_ABS_ERROR(limitFunction.getMinDot(theta), adjustmentDot, ACCEPTABLE_ERROR); // adjustmentDot at theta + QVERIFY(minDots[indexA] < adjustmentDot); // indexA should be less than minDot + QVERIFY(minDots[indexB] > adjustmentDot); // indexB should be larger than minDot + QVERIFY(minDots[indexB] < referenceDots[indexB]); // indexB should be less than what it was + + limitFunction.dynamicallyAdjustMinDots(theta, interpolatedDot + 0.01f); // reset with something larger + QCOMPARE_WITH_ABS_ERROR(limitFunction.getMinDot(theta), interpolatedDot, ACCEPTABLE_ERROR); // restored + QCOMPARE_WITH_ABS_ERROR(minDots[indexA], referenceDots[indexA], ACCEPTABLE_ERROR); // indexA is restored + QCOMPARE_WITH_ABS_ERROR(minDots[indexB], referenceDots[indexB], ACCEPTABLE_ERROR); // indexB is restored + } + { // halfway between first two data points (boundary condition) + indexA = 0; + indexB = 1; + int indexZ = minDots.size() - 1; // far boundary condition + float theta = deltaTheta * 0.5f * (float)(indexA + indexB); // halfway between two points + float interpolatedDot = limitFunction.getMinDot(theta); + float deltaDot = adjustmentDot - interpolatedDot; + limitFunction.dynamicallyAdjustMinDots(theta, adjustmentDot); + QCOMPARE_WITH_ABS_ERROR(limitFunction.getMinDot(theta), adjustmentDot, ACCEPTABLE_ERROR); // adjustmentDot at theta + QCOMPARE_WITH_ABS_ERROR(minDots[indexA], referenceDots[indexA] + deltaDot, ACCEPTABLE_ERROR); // indexA has changed + QCOMPARE_WITH_ABS_ERROR(minDots[indexB], referenceDots[indexB] + deltaDot, ACCEPTABLE_ERROR); // indexB has changed + QCOMPARE_WITH_ABS_ERROR(minDots[indexZ], referenceDots[indexZ] + deltaDot, ACCEPTABLE_ERROR); // indexZ has changed + + limitFunction.dynamicallyAdjustMinDots(theta, interpolatedDot + 0.01f); // reset with something larger + QCOMPARE_WITH_ABS_ERROR(limitFunction.getMinDot(theta), interpolatedDot, ACCEPTABLE_ERROR); // restored + QCOMPARE_WITH_ABS_ERROR(minDots[indexA], referenceDots[indexA], ACCEPTABLE_ERROR); // indexA is restored + QCOMPARE_WITH_ABS_ERROR(minDots[indexB], referenceDots[indexB], ACCEPTABLE_ERROR); // indexB is restored + QCOMPARE_WITH_ABS_ERROR(minDots[indexZ], referenceDots[indexZ], ACCEPTABLE_ERROR); // indexZ is restored + } + { // halfway between first two data points (boundary condition) + indexB = minDots.size() - 1; + indexA = indexB - 1; + int indexZ = 0; // far boundary condition + float theta = deltaTheta * 0.5f * (float)(indexA + indexB); // halfway between two points + float interpolatedDot = limitFunction.getMinDot(theta); + float deltaDot = adjustmentDot - interpolatedDot; + limitFunction.dynamicallyAdjustMinDots(theta, adjustmentDot); + QCOMPARE_WITH_ABS_ERROR(limitFunction.getMinDot(theta), adjustmentDot, ACCEPTABLE_ERROR); // adjustmentDot at theta + QCOMPARE_WITH_ABS_ERROR(minDots[indexA], referenceDots[indexA] + deltaDot, ACCEPTABLE_ERROR); // indexA has changed + QCOMPARE_WITH_ABS_ERROR(minDots[indexB], referenceDots[indexB] + deltaDot, ACCEPTABLE_ERROR); // indexB has changed + QCOMPARE_WITH_ABS_ERROR(minDots[indexZ], referenceDots[indexZ] + deltaDot, ACCEPTABLE_ERROR); // indexZ has changed + + limitFunction.dynamicallyAdjustMinDots(theta, interpolatedDot + 0.01f); // reset with something larger + QCOMPARE_WITH_ABS_ERROR(limitFunction.getMinDot(theta), interpolatedDot, ACCEPTABLE_ERROR); // restored + QCOMPARE_WITH_ABS_ERROR(minDots[indexA], referenceDots[indexA], ACCEPTABLE_ERROR); // indexA is restored + QCOMPARE_WITH_ABS_ERROR(minDots[indexB], referenceDots[indexB], ACCEPTABLE_ERROR); // indexB is restored + QCOMPARE_WITH_ABS_ERROR(minDots[indexZ], referenceDots[indexZ], ACCEPTABLE_ERROR); // indexZ is restored + } + } +} + +void RotationConstraintTests::testDynamicSwingTwistConstraint() { + + const float ACCEPTABLE_ERROR = 1.0e-6f; + + // referenceRotation is the default rotation + float referenceAngle = 1.23f; + glm::vec3 referenceAxis = glm::normalize(glm::vec3(1.0f, 2.0f, -3.0f)); + glm::quat referenceRotation = glm::angleAxis(referenceAngle, referenceAxis); + + // the angle limits of the constriant about the hinge axis + float minTwistAngle = -PI / 2.0f; + float maxTwistAngle = PI / 2.0f; + + // build the constraint + SwingTwistConstraint shoulder; + shoulder.setReferenceRotation(referenceRotation); + shoulder.setTwistLimits(minTwistAngle, maxTwistAngle); + std::vector minDots; + const float MIN_DOT = 0.5f; + minDots.push_back(MIN_DOT); + shoulder.setSwingLimits(minDots); + + // verify resolution of the swing limits + const std::vector& shoulderMinDots = shoulder.getMinDots(); + const int MIN_NUM_DOTS = 8; + int numDots = shoulderMinDots.size(); + QVERIFY(numDots >= MIN_NUM_DOTS); + + // verify values of the swing limits + QCOMPARE_WITH_ABS_ERROR(shoulderMinDots[0], shoulderMinDots[numDots - 1], ACCEPTABLE_ERROR); // endpoints should be the same + for (int i = 0; i < numDots; ++i) { + QCOMPARE_WITH_ABS_ERROR(shoulderMinDots[i], MIN_DOT, ACCEPTABLE_ERROR); // all values should be the same + } + + float deltaTheta = TWO_PI / (float)(numDots - 1); + float theta = 1.5f * deltaTheta; + glm::vec3 swingAxis(cosf(theta), 0.0f, sinf(theta)); + float deltaSwing = 0.1f; + + { // compute rotation that should NOT be constrained + float swingAngle = acosf(MIN_DOT) - deltaSwing; + glm::quat swingRotation = glm::angleAxis(swingAngle, swingAxis); + glm::quat totalRotation = swingRotation * referenceRotation; + + // verify rotation is NOT constrained + glm::quat constrainedRotation = totalRotation; + QVERIFY(!shoulder.apply(constrainedRotation)); + } + + { // compute a rotation that should be barely constrained + float swingAngle = acosf(MIN_DOT) + deltaSwing; + glm::quat swingRotation = glm::angleAxis(swingAngle, swingAxis); + glm::quat totalRotation = swingRotation * referenceRotation; + + // verify rotation is constrained + glm::quat constrainedRotation = totalRotation; + QVERIFY(shoulder.apply(constrainedRotation)); // should FAIL + } + + { // make a dynamic adjustment to the swing limits + const float SMALLER_MIN_DOT = -0.5f; + float swingAngle = acosf(SMALLER_MIN_DOT); + glm::quat swingRotation = glm::angleAxis(swingAngle, swingAxis); + glm::quat badRotation = swingRotation * referenceRotation; + + { // verify rotation is constrained + glm::quat constrainedRotation = badRotation; + QVERIFY(shoulder.apply(constrainedRotation)); + + // now poke the SMALLER_MIN_DOT into the swing limits + shoulder.dynamicallyAdjustLimits(badRotation); + + // verify that if rotation is constrained then it is only by a little bit + constrainedRotation = badRotation; + bool constrained = shoulder.apply(constrainedRotation); + if (constrained) { + // Note: Q1 = dQ * Q0 --> dQ = Q1 * Q0^ + glm::quat dQ = constrainedRotation * glm::inverse(badRotation); + const float acceptableClampAngle = 0.01f; + float deltaAngle = glm::angle(dQ); + QVERIFY(deltaAngle < acceptableClampAngle); + } + } + + { // verify that other swing axes still use the old non-adjusted limits + float deltaTheta = TWO_PI / (float)(numDots - 1); + float otherTheta = 3.5f * deltaTheta; + glm::vec3 otherSwingAxis(cosf(otherTheta), 0.0f, sinf(otherTheta)); + + { // inside rotations should be unconstrained + float goodAngle = acosf(MIN_DOT) - deltaSwing; + glm::quat goodRotation = glm::angleAxis(goodAngle, otherSwingAxis) * referenceRotation; + QVERIFY(!shoulder.apply(goodRotation)); + } + { // outside rotations should be constrained + float badAngle = acosf(MIN_DOT) + deltaSwing; + glm::quat otherBadRotation = glm::angleAxis(badAngle, otherSwingAxis) * referenceRotation; + QVERIFY(shoulder.apply(otherBadRotation)); + + float constrainedAngle = glm::angle(otherBadRotation); + QCOMPARE_WITH_ABS_ERROR(constrainedAngle, acosf(MIN_DOT), 0.1f * deltaSwing); + } + } + + { // clear dynamic adjustment + float goodAngle = acosf(MIN_DOT) - deltaSwing; + glm::quat goodRotation = glm::angleAxis(goodAngle, swingAxis) * referenceRotation; + + // when we update with a goodRotation the dynamic adjustment is cleared + shoulder.dynamicallyAdjustLimits(goodRotation); + + // verify that the old badRotation, which was not constrained dynamically, is now constrained + glm::quat constrainedRotation = badRotation; + QVERIFY(shoulder.apply(constrainedRotation)); + + // and the good rotation should not be constrained + constrainedRotation = goodRotation; + QVERIFY(!shoulder.apply(constrainedRotation)); + } + } +} + diff --git a/tests/animation/src/RotationConstraintTests.h b/tests/animation/src/RotationConstraintTests.h index 4fed3588e4..7c6d80d3eb 100644 --- a/tests/animation/src/RotationConstraintTests.h +++ b/tests/animation/src/RotationConstraintTests.h @@ -15,10 +15,12 @@ class RotationConstraintTests : public QObject { Q_OBJECT - + private slots: void testElbowConstraint(); void testSwingTwistConstraint(); + void testDynamicSwingLimitFunction(); + void testDynamicSwingTwistConstraint(); }; #endif // hifi_RotationConstraintTests_h