add unit tests for RotationConstraints

tests/animation/src/RotationConstraintTests.cpp

@@ -0,0 +1,331 @@
+//
+//  RotationConstraintTests.cpp
+//  tests/rig/src
+//
+//  Copyright 2015 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#include "RotationConstraintTests.h"
+
+#include <glm/glm.hpp>
+
+#include <ElbowConstraint.h>
+#include <NumericalConstants.h>
+#include <SwingTwistConstraint.h>
+
+// HACK -- these helper functions need to be defined BEFORE including magic inside QTestExtensions.h
+// TODO: fix QTestExtensions so we don't need to do this in every test.
+
+// Computes the error value between two quaternions (using glm::dot)
+float getErrorDifference(const glm::quat& a, const glm::quat& b) {
+        return fabsf(glm::dot(a, b)) - 1.0f;
+}       
+
+QTextStream& operator<<(QTextStream& stream, const glm::quat& q) {
+        return stream << "glm::quat { " << q.x << ", " << q.y << ", " << q.z << ", " << q.w << " }";
+} 
+
+// Produces a relative error test for float usable QCOMPARE_WITH_LAMBDA.
+inline auto errorTest (float actual, float expected, float acceptableRelativeError)
+-> std::function<bool ()> {
+    return [&actual, &expected, acceptableRelativeError] () {
+        if (expected <= acceptableRelativeError) {
+            return fabsf(actual - expected) < acceptableRelativeError;
+        }
+        return fabsf(actual - expected) / expected < acceptableRelativeError;
+    };
+}
+
+#include "../QTestExtensions.h"
+
+#define QCOMPARE_WITH_RELATIVE_ERROR(actual, expected, relativeError) \
+    QCOMPARE_WITH_LAMBDA(actual, expected, errorTest(actual, expected, relativeError))
+
+
+QTEST_MAIN(RotationConstraintTests)
+
+void RotationConstraintTests::testElbowConstraint() {
+    // 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);
+
+    // NOTE: hingeAxis is in the "referenceFrame"
+    glm::vec3 hingeAxis = glm::vec3(1.0f, 0.0f, 0.0f);
+
+    // the angle limits of the constriant about the hinge axis
+    float minAngle = -PI / 4.0f;
+    float maxAngle = PI / 3.0f;
+
+    // build the constraint
+    ElbowConstraint elbow;
+    elbow.setReferenceRotation(referenceRotation);
+    elbow.setHingeAxis(hingeAxis);
+    elbow.setAngleLimits(minAngle, maxAngle);
+
+    float smallAngle = PI / 100.0f;
+
+    { // test reference rotation -- should be unconstrained
+        glm::quat inputRotation = referenceRotation;
+        glm::quat outputRotation = inputRotation;
+        bool updated = elbow.apply(outputRotation);
+        QVERIFY(updated == false);
+        glm::quat expectedRotation = referenceRotation;
+        QCOMPARE_WITH_ABS_ERROR(expectedRotation, outputRotation, EPSILON);
+    }
+
+    { // test several simple rotations that are INSIDE the limits --  should be unconstrained
+        int numChecks = 10;
+        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;
+            bool updated = elbow.apply(outputRotation);
+            QVERIFY(updated == false);
+            QCOMPARE_WITH_ABS_ERROR(inputRotation, outputRotation, EPSILON);
+        }
+    }
+
+    { // test simple rotation just OUTSIDE minAngle --  should be constrained
+        float angle = minAngle - smallAngle;
+        glm::quat inputRotation = glm::angleAxis(angle, hingeAxis) * referenceRotation;
+        glm::quat outputRotation = inputRotation;
+        bool updated = elbow.apply(outputRotation);
+        QVERIFY(updated == true);
+        glm::quat expectedRotation = glm::angleAxis(minAngle, hingeAxis) * referenceRotation;
+        QCOMPARE_WITH_ABS_ERROR(expectedRotation, outputRotation, EPSILON);
+    }
+
+    { // test simple rotation just OUTSIDE maxAngle --  should be constrained
+        float angle = maxAngle + smallAngle;
+        glm::quat inputRotation = glm::angleAxis(angle, hingeAxis) * referenceRotation;
+        glm::quat outputRotation = inputRotation;
+        bool updated = elbow.apply(outputRotation);
+        QVERIFY(updated == true);
+        glm::quat expectedRotation = glm::angleAxis(maxAngle, hingeAxis) * referenceRotation;
+        QCOMPARE_WITH_ABS_ERROR(expectedRotation, outputRotation, EPSILON);
+    }
+
+    { // test simple twist rotation that has no hinge component --  should be constrained
+        glm::vec3 someVector(7.0f, -5.0f, 2.0f);
+        glm::vec3 twistVector = glm::normalize(glm::cross(hingeAxis, someVector));
+        float someAngle = 0.789f;
+        glm::quat inputRotation = glm::angleAxis(someAngle, twistVector) * referenceRotation;
+        glm::quat outputRotation = inputRotation;
+        bool updated = elbow.apply(outputRotation);
+        QVERIFY(updated == true);
+        glm::quat expectedRotation = referenceRotation;
+        QCOMPARE_WITH_ABS_ERROR(expectedRotation, outputRotation, EPSILON);
+    }
+}
+
+void RotationConstraintTests::testSwingTwistConstraint() {
+    // 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);
+    float lowDot = 0.25f;
+    float highDot = 0.75f;
+    // 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:
+    //
+    //          |
+    //     0.75 -               o---o---o---o
+    //          |              /             '
+    //          |             /               '
+    //          |            /                 '
+    //     0.25 o---o---o---o                   o
+    //          |
+    //          +-------+-------+-------+-------+---
+    //          0     pi/2     pi     3pi/2    2pi
+
+    int numDots = 8;
+    std::vector<float> minDots;
+    int dotIndex = 0;
+    while (dotIndex < numDots / 2) {
+        ++dotIndex;
+        minDots.push_back(lowDot);
+    }
+    while (dotIndex < numDots) {
+        minDots.push_back(highDot);
+        ++dotIndex;
+    }
+    shoulder.setSwingLimits(minDots);
+    const SwingTwistConstraint::SwingLimitFunction& shoulderSwingLimitFunction = shoulder.getSwingLimitFunction();
+
+    { // test interpolation of SwingLimitFunction
+        float theta = 0.0f;
+        float minDot = shoulderSwingLimitFunction.getMinDot(theta);
+        float expectedMinDot = lowDot;
+        QCOMPARE_WITH_RELATIVE_ERROR(minDot, expectedMinDot, EPSILON);
+
+        theta = PI;
+        minDot = shoulderSwingLimitFunction.getMinDot(theta);
+        expectedMinDot = highDot;
+        QCOMPARE_WITH_RELATIVE_ERROR(minDot, expectedMinDot, EPSILON);
+
+        // test interpolation on upward slope
+        theta = PI * (7.0f / 8.0f);
+        minDot = shoulderSwingLimitFunction.getMinDot(theta);
+        expectedMinDot = 0.5f * (highDot + lowDot);
+        QCOMPARE_WITH_RELATIVE_ERROR(minDot, expectedMinDot, EPSILON);
+
+        // test interpolation on downward slope
+        theta = PI * (15.0f / 8.0f);
+        minDot = shoulderSwingLimitFunction.getMinDot(theta);
+        expectedMinDot = 0.5f * (highDot + lowDot);
+    }
+
+    float smallAngle = PI / 100.0f;
+
+    // Note: the twist is always about the yAxis
+    glm::vec3 yAxis(0.0f, 1.0f, 0.0f);
+
+    { // test INSIDE both twist and swing
+        int numSwingAxes = 7;
+        float deltaTheta = TWO_PI / numSwingAxes;
+
+        int numTwists = 2;
+        float startTwist = minTwistAngle + smallAngle;
+        float endTwist = maxTwistAngle - smallAngle;
+        float deltaTwist = (endTwist - startTwist) / (float)(numTwists - 1);
+        float twist = startTwist;
+
+        for (int i = 0; i < numTwists; ++i) {
+            glm::quat twistRotation = glm::angleAxis(twist, yAxis);
+
+            for (float theta = 0.0f; theta < TWO_PI; theta += deltaTheta) {
+                float swing = acosf(shoulderSwingLimitFunction.getMinDot(theta)) - smallAngle;
+                glm::vec3 swingAxis(cosf(theta), 0.0f, -sinf(theta));
+                glm::quat swingRotation = glm::angleAxis(swing, swingAxis);
+
+                glm::quat inputRotation = swingRotation * twistRotation * referenceRotation;
+                glm::quat outputRotation = inputRotation;
+
+                bool updated = shoulder.apply(outputRotation);
+                QVERIFY(updated == false);
+                QCOMPARE_WITH_ABS_ERROR(inputRotation, outputRotation, EPSILON);
+            }
+            twist += deltaTwist;
+        }
+    }
+
+    { // test INSIDE twist but OUTSIDE swing
+        int numSwingAxes = 7;
+        float deltaTheta = TWO_PI / numSwingAxes;
+
+        int numTwists = 2;
+        float startTwist = minTwistAngle + smallAngle;
+        float endTwist = maxTwistAngle - smallAngle;
+        float deltaTwist = (endTwist - startTwist) / (float)(numTwists - 1);
+        float twist = startTwist;
+
+        for (int i = 0; i < numTwists; ++i) {
+            glm::quat twistRotation = glm::angleAxis(twist, yAxis);
+
+            for (float theta = 0.0f; theta < TWO_PI; theta += deltaTheta) {
+                float maxSwingAngle = acosf(shoulderSwingLimitFunction.getMinDot(theta));
+                float swing = maxSwingAngle + smallAngle;
+                glm::vec3 swingAxis(cosf(theta), 0.0f, -sinf(theta));
+                glm::quat swingRotation = glm::angleAxis(swing, swingAxis);
+
+                glm::quat inputRotation = swingRotation * twistRotation * referenceRotation;
+                glm::quat outputRotation = inputRotation;
+
+                bool updated = shoulder.apply(outputRotation);
+                QVERIFY(updated == true);
+
+                glm::quat expectedSwingRotation = glm::angleAxis(maxSwingAngle, swingAxis);
+                glm::quat expectedRotation = expectedSwingRotation * twistRotation * referenceRotation;
+                QCOMPARE_WITH_ABS_ERROR(expectedRotation, outputRotation, EPSILON);
+            }
+            twist += deltaTwist;
+        }
+    }
+
+    { // test OUTSIDE twist but INSIDE swing
+        int numSwingAxes = 6;
+        float deltaTheta = TWO_PI / numSwingAxes;
+
+        int numTwists = 2;
+        float startTwist = minTwistAngle - smallAngle;
+        float endTwist = maxTwistAngle + smallAngle;
+        float deltaTwist = (endTwist - startTwist) / (float)(numTwists - 1);
+        float twist = startTwist;
+
+        for (int i = 0; i < numTwists; ++i) {
+            glm::quat twistRotation = glm::angleAxis(twist, yAxis);
+            float clampedTwistAngle = std::min(maxTwistAngle, std::max(minTwistAngle, twist));
+
+            for (float theta = 0.0f; theta < TWO_PI; theta += deltaTheta) {
+                float maxSwingAngle = acosf(shoulderSwingLimitFunction.getMinDot(theta));
+                float swing = maxSwingAngle - smallAngle;
+                glm::vec3 swingAxis(cosf(theta), 0.0f, -sinf(theta));
+                glm::quat swingRotation = glm::angleAxis(swing, swingAxis);
+
+                glm::quat inputRotation = swingRotation * twistRotation * referenceRotation;
+                glm::quat outputRotation = inputRotation;
+
+                bool updated = shoulder.apply(outputRotation);
+                QVERIFY(updated == true);
+
+                glm::quat expectedTwistRotation = glm::angleAxis(clampedTwistAngle, yAxis);
+                glm::quat expectedRotation = swingRotation * expectedTwistRotation * referenceRotation;
+                QCOMPARE_WITH_ABS_ERROR(expectedRotation, outputRotation, EPSILON);
+            }
+            twist += deltaTwist;
+        }
+    }
+
+    { // test OUTSIDE both twist and swing
+        int numSwingAxes = 5;
+        float deltaTheta = TWO_PI / numSwingAxes;
+
+        int numTwists = 2;
+        float startTwist = minTwistAngle - smallAngle;
+        float endTwist = maxTwistAngle + smallAngle;
+        float deltaTwist = (endTwist - startTwist) / (float)(numTwists - 1);
+        float twist = startTwist;
+
+        for (int i = 0; i < numTwists; ++i) {
+            glm::quat twistRotation = glm::angleAxis(twist, yAxis);
+            float clampedTwistAngle = std::min(maxTwistAngle, std::max(minTwistAngle, twist));
+
+            for (float theta = 0.0f; theta < TWO_PI; theta += deltaTheta) {
+                float maxSwingAngle = acosf(shoulderSwingLimitFunction.getMinDot(theta));
+                float swing = maxSwingAngle + smallAngle;
+                glm::vec3 swingAxis(cosf(theta), 0.0f, -sinf(theta));
+                glm::quat swingRotation = glm::angleAxis(swing, swingAxis);
+
+                glm::quat inputRotation = swingRotation * twistRotation * referenceRotation;
+                glm::quat outputRotation = inputRotation;
+
+                bool updated = shoulder.apply(outputRotation);
+                QVERIFY(updated == true);
+
+                glm::quat expectedTwistRotation = glm::angleAxis(clampedTwistAngle, yAxis);
+                glm::quat expectedSwingRotation = glm::angleAxis(maxSwingAngle, swingAxis);
+                glm::quat expectedRotation = expectedSwingRotation * expectedTwistRotation * referenceRotation;
+                QCOMPARE_WITH_ABS_ERROR(expectedRotation, outputRotation, EPSILON);
+            }
+            twist += deltaTwist;
+        }
+    }
+}
+

tests/animation/src/RotationConstraintTests.h

@@ -0,0 +1,24 @@
+//
+//  RotationConstraintTests.h
+//  tests/rig/src
+//
+//  Copyright 2015 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#ifndef hifi_RotationConstraintTests_h
+#define hifi_RotationConstraintTests_h
+
+#include <QtTest/QtTest>
+
+class RotationConstraintTests : public QObject {
+    Q_OBJECT
+    
+private slots:
+    void testElbowConstraint();
+    void testSwingTwistConstraint();
+};
+
+#endif // hifi_RotationConstraintTests_h