From df9ccf76ab98d56252a78d3df7b6edd583e3d360 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 10 Mar 2016 13:48:27 -0800 Subject: [PATCH 1/7] fix animation-tests --- .../animation/src/AnimInverseKinematics.cpp | 7 + .../src/AnimInverseKinematicsTests.cpp | 123 +++++++++++------- tests/animation/src/AnimTests.cpp | 62 ++++----- 3 files changed, 114 insertions(+), 78 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 6a29ad61ac..4a6c3d819c 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -22,7 +22,13 @@ AnimInverseKinematics::AnimInverseKinematics(const QString& id) : AnimNode(AnimN } AnimInverseKinematics::~AnimInverseKinematics() { + std::cout << "adebug dtor" << std::endl; // adebug clearConstraints(); + std::cout << "adebug dtor 002" << std::endl; // adebug + _accumulators.clear(); + std::cout << "adebug dtor 003 targetVarVec.size() = " << _targetVarVec.size() << std::endl; // adebug + _targetVarVec.clear(); + std::cout << "adebug dtor 004 targetVarVec.size() = " << _targetVarVec.size() << std::endl; // adebug } void AnimInverseKinematics::loadDefaultPoses(const AnimPoseVec& poses) { @@ -485,6 +491,7 @@ RotationConstraint* AnimInverseKinematics::getConstraint(int index) { } void AnimInverseKinematics::clearConstraints() { + std::cout << "adebug clearConstraints size = " << _constraints.size() << std::endl; // adebug std::map::iterator constraintItr = _constraints.begin(); while (constraintItr != _constraints.end()) { delete constraintItr->second; diff --git a/tests/animation/src/AnimInverseKinematicsTests.cpp b/tests/animation/src/AnimInverseKinematicsTests.cpp index bb15a1d257..2b10892f82 100644 --- a/tests/animation/src/AnimInverseKinematicsTests.cpp +++ b/tests/animation/src/AnimInverseKinematicsTests.cpp @@ -29,7 +29,7 @@ const glm::quat identity = glm::quat(); const glm::quat quaterTurnAroundZ = glm::angleAxis(0.5f * PI, zAxis); -void makeTestFBXJoints(std::vector& fbxJoints) { +void makeTestFBXJoints(FBXGeometry& geometry) { FBXJoint joint; joint.isFree = false; joint.freeLineage.clear(); @@ -61,29 +61,29 @@ void makeTestFBXJoints(std::vector& fbxJoints) { joint.name = "A"; joint.parentIndex = -1; joint.translation = origin; - fbxJoints.push_back(joint); + geometry.joints.push_back(joint); joint.name = "B"; joint.parentIndex = 0; joint.translation = xAxis; - fbxJoints.push_back(joint); + geometry.joints.push_back(joint); joint.name = "C"; joint.parentIndex = 1; joint.translation = xAxis; - fbxJoints.push_back(joint); + geometry.joints.push_back(joint); joint.name = "D"; joint.parentIndex = 2; joint.translation = xAxis; - fbxJoints.push_back(joint); + geometry.joints.push_back(joint); // compute each joint's transform - for (int i = 1; i < (int)fbxJoints.size(); ++i) { - FBXJoint& j = fbxJoints[i]; + for (int i = 1; i < (int)geometry.joints.size(); ++i) { + FBXJoint& j = geometry.joints[i]; int parentIndex = j.parentIndex; // World = ParentWorld * T * (Roff * Rp) * Rpre * R * Rpost * (Rp-1 * Soff * Sp * S * Sp-1) - j.transform = fbxJoints[parentIndex].transform * + j.transform = geometry.joints[parentIndex].transform * glm::translate(j.translation) * j.preTransform * glm::mat4_cast(j.preRotation * j.rotation * j.postRotation) * @@ -94,14 +94,14 @@ void makeTestFBXJoints(std::vector& fbxJoints) { } void AnimInverseKinematicsTests::testSingleChain() { - std::vector fbxJoints; - makeTestFBXJoints(fbxJoints); + FBXGeometry geometry; + makeTestFBXJoints(geometry); // create a skeleton and doll AnimPose offset; - AnimSkeleton* skeleton = new AnimSkeleton(fbxJoints, offset); - AnimSkeleton::Pointer skeletonPtr(skeleton); + AnimSkeleton::Pointer skeletonPtr = std::make_shared(geometry); AnimInverseKinematics ikDoll("doll"); + ikDoll.setSkeleton(skeletonPtr); { // easy test IK of joint C @@ -113,11 +113,11 @@ void AnimInverseKinematicsTests::testSingleChain() { pose.rot = identity; pose.trans = origin; - std::vector poses; + AnimPoseVec poses; poses.push_back(pose); pose.trans = xAxis; - for (int i = 1; i < (int)fbxJoints.size(); ++i) { + for (int i = 1; i < (int)geometry.joints.size(); ++i) { poses.push_back(pose); } ikDoll.loadPoses(poses); @@ -134,7 +134,8 @@ void AnimInverseKinematicsTests::testSingleChain() { AnimVariantMap varMap; varMap.set("positionD", targetPosition); varMap.set("rotationD", targetRotation); - ikDoll.setTargetVars("D", "positionD", "rotationD"); + varMap.set("targetType", (int)IKTarget::Type::RotationAndPosition); + ikDoll.setTargetVars(QString("D"), QString("positionD"), QString("rotationD"), QString("targetType")); AnimNode::Triggers triggers; // the IK solution should be: @@ -144,38 +145,49 @@ void AnimInverseKinematicsTests::testSingleChain() { // | // A------>B------>C // + + // iterate several times float dt = 1.0f; - ikDoll.evaluate(varMap, dt, triggers); + ikDoll.overlay(varMap, dt, triggers, poses); + ikDoll.overlay(varMap, dt, triggers, poses); + ikDoll.overlay(varMap, dt, triggers, poses); + + ikDoll.overlay(varMap, dt, triggers, poses); + ikDoll.overlay(varMap, dt, triggers, poses); + ikDoll.overlay(varMap, dt, triggers, poses); + const AnimPoseVec& relativePoses = ikDoll.overlay(varMap, dt, triggers, poses); // verify absolute results // NOTE: since we expect this solution to converge very quickly (one loop) // we can impose very tight error thresholds. - std::vector absolutePoses; + AnimPoseVec absolutePoses; + for (auto pose : poses) { + absolutePoses.push_back(pose); + } ikDoll.computeAbsolutePoses(absolutePoses); - float acceptableAngle = 0.0001f; - QCOMPARE_QUATS(absolutePoses[0].rot, identity, acceptableAngle); - QCOMPARE_QUATS(absolutePoses[1].rot, identity, acceptableAngle); - QCOMPARE_QUATS(absolutePoses[2].rot, quaterTurnAroundZ, acceptableAngle); - QCOMPARE_QUATS(absolutePoses[3].rot, quaterTurnAroundZ, acceptableAngle); + const float acceptableAngleError = 0.001f; + QCOMPARE_QUATS(absolutePoses[0].rot, identity, acceptableAngleError); + QCOMPARE_QUATS(absolutePoses[1].rot, identity, acceptableAngleError); + QCOMPARE_QUATS(absolutePoses[2].rot, quaterTurnAroundZ, acceptableAngleError); + QCOMPARE_QUATS(absolutePoses[3].rot, quaterTurnAroundZ, acceptableAngleError); - QCOMPARE_WITH_ABS_ERROR(absolutePoses[0].trans, origin, EPSILON); - QCOMPARE_WITH_ABS_ERROR(absolutePoses[1].trans, xAxis, EPSILON); - QCOMPARE_WITH_ABS_ERROR(absolutePoses[2].trans, 2.0f * xAxis, EPSILON); - QCOMPARE_WITH_ABS_ERROR(absolutePoses[3].trans, targetPosition, EPSILON); + const float acceptableTranslationError = 0.025f; + QCOMPARE_WITH_ABS_ERROR(absolutePoses[0].trans, origin, acceptableTranslationError); + QCOMPARE_WITH_ABS_ERROR(absolutePoses[1].trans, xAxis, acceptableTranslationError); + QCOMPARE_WITH_ABS_ERROR(absolutePoses[2].trans, 2.0f * xAxis, acceptableTranslationError); + QCOMPARE_WITH_ABS_ERROR(absolutePoses[3].trans, targetPosition, acceptableTranslationError); // verify relative results - const std::vector& relativePoses = ikDoll.getRelativePoses(); - QCOMPARE_QUATS(relativePoses[0].rot, identity, acceptableAngle); - QCOMPARE_QUATS(relativePoses[1].rot, identity, acceptableAngle); - QCOMPARE_QUATS(relativePoses[2].rot, quaterTurnAroundZ, acceptableAngle); - QCOMPARE_QUATS(relativePoses[3].rot, identity, acceptableAngle); + QCOMPARE_QUATS(relativePoses[0].rot, identity, acceptableAngleError); + QCOMPARE_QUATS(relativePoses[1].rot, identity, acceptableAngleError); + QCOMPARE_QUATS(relativePoses[2].rot, quaterTurnAroundZ, acceptableAngleError); + QCOMPARE_QUATS(relativePoses[3].rot, identity, acceptableAngleError); - QCOMPARE_WITH_ABS_ERROR(relativePoses[0].trans, origin, EPSILON); - QCOMPARE_WITH_ABS_ERROR(relativePoses[1].trans, xAxis, EPSILON); - QCOMPARE_WITH_ABS_ERROR(relativePoses[2].trans, xAxis, EPSILON); - QCOMPARE_WITH_ABS_ERROR(relativePoses[3].trans, xAxis, EPSILON); + QCOMPARE_WITH_ABS_ERROR(relativePoses[0].trans, origin, acceptableTranslationError); + QCOMPARE_WITH_ABS_ERROR(relativePoses[1].trans, xAxis, acceptableTranslationError); + QCOMPARE_WITH_ABS_ERROR(relativePoses[2].trans, xAxis, acceptableTranslationError); + QCOMPARE_WITH_ABS_ERROR(relativePoses[3].trans, xAxis, acceptableTranslationError); } - { // hard test IK of joint C // load intial poses that look like this: // @@ -188,8 +200,8 @@ void AnimInverseKinematicsTests::testSingleChain() { pose.scale = glm::vec3(1.0f); pose.rot = identity; pose.trans = origin; - - std::vector poses; + + AnimPoseVec poses; poses.push_back(pose); pose.trans = xAxis; @@ -211,15 +223,26 @@ void AnimInverseKinematicsTests::testSingleChain() { AnimVariantMap varMap; varMap.set("positionD", targetPosition); varMap.set("rotationD", targetRotation); - ikDoll.setTargetVars("D", "positionD", "rotationD"); + varMap.set("targetType", (int)IKTarget::Type::RotationAndPosition); + ikDoll.setTargetVars(QString("D"), QString("positionD"), QString("rotationD"), QString("targetType")); AnimNode::Triggers triggers; // the IK solution should be: // // A------>B------>C------>D // + + // iterate several times float dt = 1.0f; - ikDoll.evaluate(varMap, dt, triggers); + ikDoll.overlay(varMap, dt, triggers, poses); + ikDoll.overlay(varMap, dt, triggers, poses); + ikDoll.overlay(varMap, dt, triggers, poses); + ikDoll.overlay(varMap, dt, triggers, poses); + ikDoll.overlay(varMap, dt, triggers, poses); + ikDoll.overlay(varMap, dt, triggers, poses); + ikDoll.overlay(varMap, dt, triggers, poses); + ikDoll.overlay(varMap, dt, triggers, poses); + const AnimPoseVec& relativePoses = ikDoll.overlay(varMap, dt, triggers, poses); // verify absolute results // NOTE: the IK algorithm doesn't converge very fast for full-reach targets, @@ -228,31 +251,33 @@ void AnimInverseKinematicsTests::testSingleChain() { // NOTE: constraints may help speed up convergence since some joints may get clamped // to maximum extension. TODO: experiment with tightening the error thresholds when // constraints are working. - std::vector absolutePoses; + AnimPoseVec absolutePoses; + for (auto pose : poses) { + absolutePoses.push_back(pose); + } ikDoll.computeAbsolutePoses(absolutePoses); - float acceptableAngle = 0.1f; // radians + float acceptableAngle = 0.01f; // radians QCOMPARE_QUATS(absolutePoses[0].rot, identity, acceptableAngle); QCOMPARE_QUATS(absolutePoses[1].rot, identity, acceptableAngle); QCOMPARE_QUATS(absolutePoses[2].rot, identity, acceptableAngle); QCOMPARE_QUATS(absolutePoses[3].rot, identity, acceptableAngle); - float acceptableDistance = 0.4f; - QCOMPARE_WITH_ABS_ERROR(absolutePoses[0].trans, origin, EPSILON); + float acceptableDistance = 0.03f; + QCOMPARE_WITH_ABS_ERROR(absolutePoses[0].trans, origin, acceptableDistance); QCOMPARE_WITH_ABS_ERROR(absolutePoses[1].trans, xAxis, acceptableDistance); QCOMPARE_WITH_ABS_ERROR(absolutePoses[2].trans, 2.0f * xAxis, acceptableDistance); QCOMPARE_WITH_ABS_ERROR(absolutePoses[3].trans, 3.0f * xAxis, acceptableDistance); // verify relative results - const std::vector& relativePoses = ikDoll.getRelativePoses(); QCOMPARE_QUATS(relativePoses[0].rot, identity, acceptableAngle); QCOMPARE_QUATS(relativePoses[1].rot, identity, acceptableAngle); QCOMPARE_QUATS(relativePoses[2].rot, identity, acceptableAngle); QCOMPARE_QUATS(relativePoses[3].rot, identity, acceptableAngle); - QCOMPARE_WITH_ABS_ERROR(relativePoses[0].trans, origin, EPSILON); - QCOMPARE_WITH_ABS_ERROR(relativePoses[1].trans, xAxis, EPSILON); - QCOMPARE_WITH_ABS_ERROR(relativePoses[2].trans, xAxis, EPSILON); - QCOMPARE_WITH_ABS_ERROR(relativePoses[3].trans, xAxis, EPSILON); + QCOMPARE_WITH_ABS_ERROR(relativePoses[0].trans, origin, acceptableDistance); + QCOMPARE_WITH_ABS_ERROR(relativePoses[1].trans, xAxis, acceptableDistance); + QCOMPARE_WITH_ABS_ERROR(relativePoses[2].trans, xAxis, acceptableDistance); + QCOMPARE_WITH_ABS_ERROR(relativePoses[3].trans, xAxis, acceptableDistance); } } diff --git a/tests/animation/src/AnimTests.cpp b/tests/animation/src/AnimTests.cpp index 6812bb63b6..130b692fb0 100644 --- a/tests/animation/src/AnimTests.cpp +++ b/tests/animation/src/AnimTests.cpp @@ -38,8 +38,9 @@ void AnimTests::testClipInternalState() { float endFrame = 20.0f; float timeScale = 1.1f; bool loopFlag = true; + bool mirrorFlag = false; - AnimClip clip(id, url, startFrame, endFrame, timeScale, loopFlag); + AnimClip clip(id, url, startFrame, endFrame, timeScale, loopFlag, mirrorFlag); QVERIFY(clip.getID() == id); QVERIFY(clip.getType() == AnimNode::Type::Clip); @@ -49,6 +50,7 @@ void AnimTests::testClipInternalState() { QVERIFY(clip._endFrame == endFrame); QVERIFY(clip._timeScale == timeScale); QVERIFY(clip._loopFlag == loopFlag); + QVERIFY(clip._mirrorFlag == mirrorFlag); } static float framesToSec(float secs) { @@ -62,12 +64,13 @@ void AnimTests::testClipEvaulate() { float startFrame = 2.0f; float endFrame = 22.0f; float timeScale = 1.0f; - float loopFlag = true; + bool loopFlag = true; + bool mirrorFlag = false; auto vars = AnimVariantMap(); vars.set("FalseVar", false); - AnimClip clip(id, url, startFrame, endFrame, timeScale, loopFlag); + AnimClip clip(id, url, startFrame, endFrame, timeScale, loopFlag, mirrorFlag); AnimNode::Triggers triggers; clip.evaluate(vars, framesToSec(10.0f), triggers); @@ -97,7 +100,8 @@ void AnimTests::testClipEvaulateWithVars() { float startFrame = 2.0f; float endFrame = 22.0f; float timeScale = 1.0f; - float loopFlag = true; + bool loopFlag = true; + bool mirrorFlag = false; float startFrame2 = 22.0f; float endFrame2 = 100.0f; @@ -110,7 +114,7 @@ void AnimTests::testClipEvaulateWithVars() { vars.set("timeScale2", timeScale2); vars.set("loopFlag2", loopFlag2); - AnimClip clip(id, url, startFrame, endFrame, timeScale, loopFlag); + AnimClip clip(id, url, startFrame, endFrame, timeScale, loopFlag, mirrorFlag); clip.setStartFrameVar("startFrame2"); clip.setEndFrameVar("endFrame2"); clip.setTimeScaleVar("timeScale2"); @@ -583,23 +587,23 @@ void AnimTests::testExpressionEvaluator() { TEST_BOOL_EXPR(false && false); TEST_BOOL_EXPR(false && true); - TEST_BOOL_EXPR(true || false && true); - TEST_BOOL_EXPR(true || false && false); - TEST_BOOL_EXPR(true || true && true); - TEST_BOOL_EXPR(true || true && false); - TEST_BOOL_EXPR(false || false && true); - TEST_BOOL_EXPR(false || false && false); - TEST_BOOL_EXPR(false || true && true); - TEST_BOOL_EXPR(false || true && false); + TEST_BOOL_EXPR(true || (false && true)); + TEST_BOOL_EXPR(true || (false && false)); + TEST_BOOL_EXPR(true || (true && true)); + TEST_BOOL_EXPR(true || (true && false)); + TEST_BOOL_EXPR(false || (false && true)); + TEST_BOOL_EXPR(false || (false && false)); + TEST_BOOL_EXPR(false || (true && true)); + TEST_BOOL_EXPR(false || (true && false)); - TEST_BOOL_EXPR(true && false || true); - TEST_BOOL_EXPR(true && false || false); - TEST_BOOL_EXPR(true && true || true); - TEST_BOOL_EXPR(true && true || false); - TEST_BOOL_EXPR(false && false || true); - TEST_BOOL_EXPR(false && false || false); - TEST_BOOL_EXPR(false && true || true); - TEST_BOOL_EXPR(false && true || false); + TEST_BOOL_EXPR((true && false) || true); + TEST_BOOL_EXPR((true && false) || false); + TEST_BOOL_EXPR((true && true) || true); + TEST_BOOL_EXPR((true && true) || false); + TEST_BOOL_EXPR((false && false) || true); + TEST_BOOL_EXPR((false && false) || false); + TEST_BOOL_EXPR((false && true) || true); + TEST_BOOL_EXPR((false && true) || false); TEST_BOOL_EXPR(t || false); TEST_BOOL_EXPR(t || true); @@ -610,14 +614,14 @@ void AnimTests::testExpressionEvaluator() { TEST_BOOL_EXPR(!false); TEST_BOOL_EXPR(!true || true); - TEST_BOOL_EXPR(!true && !false || !true); - TEST_BOOL_EXPR(!true && !false || true); - TEST_BOOL_EXPR(!true && false || !true); - TEST_BOOL_EXPR(!true && false || true); - TEST_BOOL_EXPR(true && !false || !true); - TEST_BOOL_EXPR(true && !false || true); - TEST_BOOL_EXPR(true && false || !true); - TEST_BOOL_EXPR(true && false || true); + TEST_BOOL_EXPR((!true && !false) || !true); + TEST_BOOL_EXPR((!true && !false) || true); + TEST_BOOL_EXPR((!true && false) || !true); + TEST_BOOL_EXPR((!true && false) || true); + TEST_BOOL_EXPR((true && !false) || !true); + TEST_BOOL_EXPR((true && !false) || true); + TEST_BOOL_EXPR((true && false) || !true); + TEST_BOOL_EXPR((true && false) || true); TEST_BOOL_EXPR(!(true && f) || !t); TEST_BOOL_EXPR(!!!(t) && (!!f || true)); From 6ebb94b1f4605cbe11a1459782a3bb67eadd1aa7 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 10 Mar 2016 13:48:32 -0800 Subject: [PATCH 2/7] minor API cleanup of SwingLimitFunction API --- libraries/animation/src/SwingTwistConstraint.cpp | 11 ++--------- libraries/animation/src/SwingTwistConstraint.h | 3 --- 2 files changed, 2 insertions(+), 12 deletions(-) diff --git a/libraries/animation/src/SwingTwistConstraint.cpp b/libraries/animation/src/SwingTwistConstraint.cpp index 3bf661612e..3a2606c5ce 100644 --- a/libraries/animation/src/SwingTwistConstraint.cpp +++ b/libraries/animation/src/SwingTwistConstraint.cpp @@ -24,15 +24,8 @@ const int LAST_CLAMP_NO_BOUNDARY = 0; const int LAST_CLAMP_HIGH_BOUNDARY = 1; SwingTwistConstraint::SwingLimitFunction::SwingLimitFunction() { - setCone(PI); -} - -void SwingTwistConstraint::SwingLimitFunction::setCone(float maxAngle) { - _minDots.clear(); - float minDot = glm::clamp(maxAngle, MIN_MINDOT, MAX_MINDOT); - _minDots.push_back(minDot); - // push the first value to the back to establish cyclic boundary conditions - _minDots.push_back(minDot); + _minDots.push_back(-1.0f); + _minDots.push_back(-1.0f); } void SwingTwistConstraint::SwingLimitFunction::setMinDots(const std::vector& minDots) { diff --git a/libraries/animation/src/SwingTwistConstraint.h b/libraries/animation/src/SwingTwistConstraint.h index f73bbfb233..620e63e98b 100644 --- a/libraries/animation/src/SwingTwistConstraint.h +++ b/libraries/animation/src/SwingTwistConstraint.h @@ -59,9 +59,6 @@ public: 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); From 749dcf2c1de2cd094bcb080a088f00ffb7421a2f Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 11 Mar 2016 10:19:50 -0800 Subject: [PATCH 3/7] dynamic adjustment for swing --- .../animation/src/SwingTwistConstraint.cpp | 167 +++++++++++- .../animation/src/SwingTwistConstraint.h | 26 +- .../animation/src/RotationConstraintTests.cpp | 253 +++++++++++++++++- tests/animation/src/RotationConstraintTests.h | 4 +- 4 files changed, 432 insertions(+), 18 deletions(-) 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 From 4b7514479759fff0a09f027d927d3f6b070a06e5 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 11 Mar 2016 10:20:40 -0800 Subject: [PATCH 4/7] remove debug cruft --- libraries/animation/src/AnimInverseKinematics.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 4a6c3d819c..46e17c39d8 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -22,13 +22,9 @@ AnimInverseKinematics::AnimInverseKinematics(const QString& id) : AnimNode(AnimN } AnimInverseKinematics::~AnimInverseKinematics() { - std::cout << "adebug dtor" << std::endl; // adebug clearConstraints(); - std::cout << "adebug dtor 002" << std::endl; // adebug _accumulators.clear(); - std::cout << "adebug dtor 003 targetVarVec.size() = " << _targetVarVec.size() << std::endl; // adebug _targetVarVec.clear(); - std::cout << "adebug dtor 004 targetVarVec.size() = " << _targetVarVec.size() << std::endl; // adebug } void AnimInverseKinematics::loadDefaultPoses(const AnimPoseVec& poses) { @@ -491,7 +487,6 @@ RotationConstraint* AnimInverseKinematics::getConstraint(int index) { } void AnimInverseKinematics::clearConstraints() { - std::cout << "adebug clearConstraints size = " << _constraints.size() << std::endl; // adebug std::map::iterator constraintItr = _constraints.begin(); while (constraintItr != _constraints.end()) { delete constraintItr->second; From c9f988d34048de746cae6db4fc15611709ad5d72 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 11 Mar 2016 11:06:58 -0800 Subject: [PATCH 5/7] dynamic adjustment for twist --- .../animation/src/SwingTwistConstraint.cpp | 120 +++++++++++------- .../animation/src/SwingTwistConstraint.h | 7 + 2 files changed, 83 insertions(+), 44 deletions(-) diff --git a/libraries/animation/src/SwingTwistConstraint.cpp b/libraries/animation/src/SwingTwistConstraint.cpp index dd0a9ce0e8..72659ff09d 100644 --- a/libraries/animation/src/SwingTwistConstraint.cpp +++ b/libraries/animation/src/SwingTwistConstraint.cpp @@ -13,6 +13,7 @@ #include #include +#include #include @@ -31,7 +32,7 @@ SwingTwistConstraint::SwingLimitFunction::SwingLimitFunction() { _minDotIndexB = -1; } -// In order to support the dynamic adjustment to swing limits we require +// 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; @@ -214,11 +215,10 @@ void SwingTwistConstraint::setSwingLimits(const std::vector& swungDir limits.reserve(numLimits); // compute the limit pairs: - const glm::vec3 yAxis = glm::vec3(0.0f, 1.0f, 0.0f); for (int i = 0; i < numLimits; ++i) { float directionLength = glm::length(swungDirections[i]); if (directionLength > EPSILON) { - glm::vec3 swingAxis = glm::cross(yAxis, swungDirections[i]); + glm::vec3 swingAxis = glm::cross(Vectors::UNIT_Y, swungDirections[i]); float theta = atan2f(-swingAxis.z, swingAxis.x); if (theta < 0.0f) { theta += TWO_PI; @@ -285,51 +285,57 @@ void SwingTwistConstraint::setTwistLimits(float minTwist, float maxTwist) { _maxTwist = glm::max(minTwist, maxTwist); _lastTwistBoundary = LAST_CLAMP_NO_BOUNDARY; + _twistAdjusted = false; +} + +// private +float SwingTwistConstraint::handleTwistBoundaryConditions(float twistAngle) const { + // 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; + } + return twistAngle; } bool SwingTwistConstraint::apply(glm::quat& rotation) const { // decompose the rotation into first twist about yAxis, then swing about something perp - const glm::vec3 yAxis(0.0f, 1.0f, 0.0f); // NOTE: rotation = postRotation * referenceRotation glm::quat postRotation = rotation * glm::inverse(_referenceRotation); glm::quat swingRotation, twistRotation; - swingTwistDecomposition(postRotation, yAxis, swingRotation, twistRotation); + swingTwistDecomposition(postRotation, Vectors::UNIT_Y, swingRotation, twistRotation); // NOTE: postRotation = swingRotation * twistRotation - // compute twistAngle + // compute raw twistAngle float twistAngle = 2.0f * acosf(fabsf(twistRotation.w)); - const glm::vec3 xAxis = glm::vec3(1.0f, 0.0f, 0.0f); - glm::vec3 twistedX = twistRotation * xAxis; - twistAngle *= copysignf(1.0f, glm::dot(glm::cross(xAxis, twistedX), yAxis)); + glm::vec3 twistedX = twistRotation * Vectors::UNIT_X; + twistAngle *= copysignf(1.0f, glm::dot(glm::cross(Vectors::UNIT_X, twistedX), Vectors::UNIT_Y)); 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; - } + // twist limits apply --> figure out which limit we're hitting, if any + twistAngle = handleTwistBoundaryConditions(twistAngle); // clamp twistAngle float clampedTwistAngle = glm::clamp(twistAngle, _minTwist, _maxTwist); @@ -346,15 +352,15 @@ bool SwingTwistConstraint::apply(glm::quat& rotation) const { // 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); + glm::vec3 swungY = swingRotation * Vectors::UNIT_Y; + glm::vec3 swingAxis = glm::cross(Vectors::UNIT_Y, swungY); float axisLength = glm::length(swingAxis); if (axisLength > EPSILON) { // The limit of swing is a function of "theta" which can be computed from the swingAxis // (which is in the constraint's ZX plane). float theta = atan2f(-swingAxis.z, swingAxis.x); float minDot = _swingLimitFunction.getMinDot(theta); - if (glm::dot(swungY, yAxis) < minDot) { + if (glm::dot(swungY, Vectors::UNIT_Y) < minDot) { // The swing limits are violated so we extract the angle from midDot and // use it to supply a new rotation. swingAxis /= axisLength; @@ -365,7 +371,7 @@ bool SwingTwistConstraint::apply(glm::quat& rotation) const { if (somethingClamped) { // update the rotation - twistRotation = glm::angleAxis(twistAngle, yAxis); + twistRotation = glm::angleAxis(twistAngle, Vectors::UNIT_Y); rotation = swingRotation * twistRotation * _referenceRotation; return true; } @@ -376,14 +382,40 @@ 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); + swingTwistDecomposition(postRotation, Vectors::UNIT_Y, swingRotation, twistRotation); - // we currently only handle swing limits - glm::vec3 swungY = swingRotation * yAxis; - glm::vec3 swingAxis = glm::cross(yAxis, swungY); + // adjust swing limits + glm::vec3 swungY = swingRotation * Vectors::UNIT_Y; + glm::vec3 swingAxis = glm::cross(Vectors::UNIT_Y, swungY); float theta = atan2f(-swingAxis.z, swingAxis.x); _swingLimitFunction.dynamicallyAdjustMinDots(theta, swungY.y); + + // restore twist limits + if (_twistAdjusted) { + _minTwist = _oldMinTwist; + _maxTwist = _oldMaxTwist; + _twistAdjusted = false; + } + + if (_minTwist != _maxTwist) { + // compute twistAngle + float twistAngle = 2.0f * acosf(fabsf(twistRotation.w)); + glm::vec3 twistedX = twistRotation * Vectors::UNIT_X; + twistAngle *= copysignf(1.0f, glm::dot(glm::cross(Vectors::UNIT_X, twistedX), Vectors::UNIT_Y)); + twistAngle = handleTwistBoundaryConditions(twistAngle); + + if (twistAngle < _minTwist || twistAngle > _maxTwist) { + // expand twist limits + _twistAdjusted = true; + _oldMinTwist = _minTwist; + _oldMaxTwist = _maxTwist; + if (twistAngle < _minTwist) { + _minTwist = twistAngle; + } else if (twistAngle > _maxTwist) { + _maxTwist = twistAngle; + } + } + } } void SwingTwistConstraint::clearHistory() { diff --git a/libraries/animation/src/SwingTwistConstraint.h b/libraries/animation/src/SwingTwistConstraint.h index df9da8cabe..4734aa8b9d 100644 --- a/libraries/animation/src/SwingTwistConstraint.h +++ b/libraries/animation/src/SwingTwistConstraint.h @@ -100,15 +100,22 @@ public: /// \brief exposed for unit testing void clearHistory(); +private: + float handleTwistBoundaryConditions(float twistAngle) const; + protected: SwingLimitFunction _swingLimitFunction; float _minTwist; float _maxTwist; + float _oldMinTwist; + float _oldMaxTwist; + // 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 }; + bool _twistAdjusted { false }; }; #endif // hifi_SwingTwistConstraint_h From 71a81331d19a80e37c3dfb774ba5a9b7db858ff2 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 11 Mar 2016 13:20:03 -0800 Subject: [PATCH 6/7] unit tests for dynamic twist limit adjustment --- .../animation/src/RotationConstraintTests.cpp | 101 +++++++++++++++++- tests/animation/src/RotationConstraintTests.h | 3 +- 2 files changed, 100 insertions(+), 4 deletions(-) diff --git a/tests/animation/src/RotationConstraintTests.cpp b/tests/animation/src/RotationConstraintTests.cpp index b0351721ae..f828201a81 100644 --- a/tests/animation/src/RotationConstraintTests.cpp +++ b/tests/animation/src/RotationConstraintTests.cpp @@ -13,6 +13,7 @@ #include #include +#include #include #include @@ -433,8 +434,7 @@ void RotationConstraintTests::testDynamicSwingLimitFunction() { } } -void RotationConstraintTests::testDynamicSwingTwistConstraint() { - +void RotationConstraintTests::testDynamicSwing() { const float ACCEPTABLE_ERROR = 1.0e-6f; // referenceRotation is the default rotation @@ -489,7 +489,7 @@ void RotationConstraintTests::testDynamicSwingTwistConstraint() { // verify rotation is constrained glm::quat constrainedRotation = totalRotation; - QVERIFY(shoulder.apply(constrainedRotation)); // should FAIL + QVERIFY(shoulder.apply(constrainedRotation)); } { // make a dynamic adjustment to the swing limits @@ -555,3 +555,98 @@ void RotationConstraintTests::testDynamicSwingTwistConstraint() { } } +void RotationConstraintTests::testDynamicTwist() { + // 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 + const float minTwistAngle = -PI / 2.0f; + const float maxTwistAngle = PI / 2.0f; + + // build the constraint + SwingTwistConstraint shoulder; + shoulder.setReferenceRotation(referenceRotation); + shoulder.setTwistLimits(minTwistAngle, maxTwistAngle); + + glm::vec3 twistAxis = Vectors::UNIT_Y; + float deltaTwist = 0.1f; + + { // compute min rotation that should NOT be constrained + float twistAngle = minTwistAngle + deltaTwist; + glm::quat twistRotation = glm::angleAxis(twistAngle, twistAxis); + glm::quat totalRotation = twistRotation * referenceRotation; + + // verify rotation is NOT constrained + glm::quat constrainedRotation = totalRotation; + QVERIFY(!shoulder.apply(constrainedRotation)); + } + { // compute max rotation that should NOT be constrained + float twistAngle = maxTwistAngle - deltaTwist; + glm::quat twistRotation = glm::angleAxis(twistAngle, twistAxis); + glm::quat totalRotation = twistRotation * referenceRotation; + + // verify rotation is NOT constrained + glm::quat constrainedRotation = totalRotation; + QVERIFY(!shoulder.apply(constrainedRotation)); + } + { // compute a min rotation that should be barely constrained + float twistAngle = minTwistAngle - deltaTwist; + glm::quat twistRotation = glm::angleAxis(twistAngle, twistAxis); + glm::quat totalRotation = twistRotation * referenceRotation; + + // verify rotation is constrained + glm::quat constrainedRotation = totalRotation; + QVERIFY(shoulder.apply(constrainedRotation)); + + // adjust the constraint and verify rotation is NOT constrained + shoulder.dynamicallyAdjustLimits(totalRotation); + constrainedRotation = totalRotation; + bool constrained = shoulder.apply(constrainedRotation); + if (constrained) { + // or, if it is constrained then the adjustment is very small + // Note: Q1 = dQ * Q0 --> dQ = Q1 * Q0^ + glm::quat dQ = constrainedRotation * glm::inverse(totalRotation); + const float acceptableClampAngle = 0.01f; + float deltaAngle = glm::angle(dQ); + QVERIFY(deltaAngle < acceptableClampAngle); + } + + // clear the adjustment using a null rotation + shoulder.dynamicallyAdjustLimits(glm::quat()); + + // verify that rotation is constrained again + constrainedRotation = totalRotation; + QVERIFY(shoulder.apply(constrainedRotation)); + } + { // compute a min rotation that should be barely constrained + float twistAngle = maxTwistAngle + deltaTwist; + glm::quat twistRotation = glm::angleAxis(twistAngle, twistAxis); + glm::quat totalRotation = twistRotation * referenceRotation; + + // verify rotation is constrained + glm::quat constrainedRotation = totalRotation; + QVERIFY(shoulder.apply(constrainedRotation)); + + // adjust the constraint and verify rotation is NOT constrained + shoulder.dynamicallyAdjustLimits(totalRotation); + constrainedRotation = totalRotation; + bool constrained = shoulder.apply(constrainedRotation); + if (constrained) { + // or, if it is constrained then the adjustment is very small + // Note: Q1 = dQ * Q0 --> dQ = Q1 * Q0^ + glm::quat dQ = constrainedRotation * glm::inverse(totalRotation); + const float acceptableClampAngle = 0.01f; + float deltaAngle = glm::angle(dQ); + QVERIFY(deltaAngle < acceptableClampAngle); + } + + // clear the adjustment using a null rotation + shoulder.dynamicallyAdjustLimits(glm::quat()); + + // verify that rotation is constrained again + constrainedRotation = totalRotation; + QVERIFY(shoulder.apply(constrainedRotation)); + } +} diff --git a/tests/animation/src/RotationConstraintTests.h b/tests/animation/src/RotationConstraintTests.h index 7c6d80d3eb..e63d08bc1f 100644 --- a/tests/animation/src/RotationConstraintTests.h +++ b/tests/animation/src/RotationConstraintTests.h @@ -20,7 +20,8 @@ private slots: void testElbowConstraint(); void testSwingTwistConstraint(); void testDynamicSwingLimitFunction(); - void testDynamicSwingTwistConstraint(); + void testDynamicSwing(); + void testDynamicTwist(); }; #endif // hifi_RotationConstraintTests_h From 48efbba335c93dd6f31f0984ad2b34148d3b3791 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Fri, 11 Mar 2016 14:44:43 -0800 Subject: [PATCH 7/7] use dynamic constraints for IK --- libraries/animation/src/AnimInverseKinematics.cpp | 11 +++++++++++ libraries/animation/src/RotationConstraint.h | 4 ++++ libraries/animation/src/SwingTwistConstraint.h | 2 +- 3 files changed, 16 insertions(+), 1 deletion(-) diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 46e17c39d8..f4df7ada82 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -396,6 +396,17 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars } _relativePoses[i].trans = underPoses[i].trans; } + + if (!_relativePoses.empty()) { + // Sometimes the underpose itself can violate the constraints. Rather than + // clamp the animation we dynamically expand each constraint to accomodate it. + std::map::iterator constraintItr = _constraints.begin(); + while (constraintItr != _constraints.end()) { + int index = constraintItr->first; + constraintItr->second->dynamicallyAdjustLimits(_relativePoses[index].rot); + ++constraintItr; + } + } } if (!_relativePoses.empty()) { diff --git a/libraries/animation/src/RotationConstraint.h b/libraries/animation/src/RotationConstraint.h index 0745500582..9e34537cab 100644 --- a/libraries/animation/src/RotationConstraint.h +++ b/libraries/animation/src/RotationConstraint.h @@ -31,6 +31,10 @@ public: /// \return true if this constraint is part of lower spine virtual bool isLowerSpine() const { return false; } + /// \param rotation rotation to allow + /// \brief clear previous adjustment and adjust constraint limits to allow rotation + virtual void dynamicallyAdjustLimits(const glm::quat& rotation) {} + protected: glm::quat _referenceRotation = glm::quat(); }; diff --git a/libraries/animation/src/SwingTwistConstraint.h b/libraries/animation/src/SwingTwistConstraint.h index 4734aa8b9d..93d7449d8f 100644 --- a/libraries/animation/src/SwingTwistConstraint.h +++ b/libraries/animation/src/SwingTwistConstraint.h @@ -55,7 +55,7 @@ public: /// \param rotation rotation to allow /// \brief clear previous adjustment and adjust constraint limits to allow rotation - void dynamicallyAdjustLimits(const glm::quat& rotation); + virtual void dynamicallyAdjustLimits(const glm::quat& rotation) override; // for testing purposes const std::vector& getMinDots() { return _swingLimitFunction.getMinDots(); }