Merge branch 'master' into 20639

interface/src/avatar/SkeletonModel.cpp

@@ -110,7 +110,7 @@ void SkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
         params.modelRotation = getRotation();
         params.modelTranslation = getTranslation();
         params.leanSideways = _owningAvatar->getHead()->getFinalLeanSideways();
-        params.leanForward = _owningAvatar->getHead()->getFinalLeanSideways();
+        params.leanForward = _owningAvatar->getHead()->getFinalLeanForward();
         params.torsoTwist = _owningAvatar->getHead()->getTorsoTwist();
         params.localHeadOrientation = _owningAvatar->getHead()->getFinalOrientationInLocalFrame();
         params.worldHeadOrientation = _owningAvatar->getHead()->getFinalOrientationInWorldFrame();

libraries/animation/src/ElbowConstraint.cpp

@@ -0,0 +1,80 @@
+//
+//  ElbowConstraint.cpp
+//
+//  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 "ElbowConstraint.h"
+
+#include <algorithm>
+
+#include <GeometryUtil.h>
+#include <NumericalConstants.h>
+
+ElbowConstraint::ElbowConstraint() :
+        _referenceRotation(),
+        _minAngle(-PI),
+        _maxAngle(PI)
+{
+}
+
+void ElbowConstraint::setHingeAxis(const glm::vec3& axis) {
+    float axisLength = glm::length(axis);
+    assert(axisLength > EPSILON);
+    _axis = axis / axisLength;
+
+    // for later we need a second axis that is perpendicular to the first
+    for (int i = 0; i < 3; ++i) {
+        float component = _axis[i];
+        const float MIN_LARGEST_NORMALIZED_VECTOR_COMPONENT = 0.57735f; // just under 1/sqrt(3)
+        if (fabsf(component) > MIN_LARGEST_NORMALIZED_VECTOR_COMPONENT) {
+           int j = (i + 1) % 3;
+           int k = (j + 1) % 3;
+           _perpAxis[i] = - _axis[j];
+           _perpAxis[j] = component;
+           _perpAxis[k] = 0.0f;
+           _perpAxis = glm::normalize(_perpAxis);
+           break;
+       }
+    }
+}
+
+void ElbowConstraint::setAngleLimits(float minAngle, float maxAngle) {
+    // NOTE: min/maxAngle angles should be in the range [-PI, PI]
+    _minAngle = glm::min(minAngle, maxAngle);
+    _maxAngle = glm::max(minAngle, maxAngle);
+}
+
+bool ElbowConstraint::apply(glm::quat& rotation) const {
+    // decompose the rotation into swing and twist
+    // NOTE: rotation = postRotation * referenceRotation
+    glm::quat postRotation = rotation * glm::inverse(_referenceRotation);
+    glm::quat swingRotation, twistRotation;
+    swingTwistDecomposition(postRotation, _axis, swingRotation, twistRotation);
+    // NOTE: postRotation = swingRotation * twistRotation
+
+    // compute twistAngle
+    float twistAngle = 2.0f * acosf(fabsf(twistRotation.w));
+    glm::vec3 twisted = twistRotation * _perpAxis;
+    twistAngle *= copysignf(1.0f, glm::dot(glm::cross(_perpAxis, twisted), _axis));
+
+    // clamp twistAngle
+    float clampedTwistAngle = glm::clamp(twistAngle, _minAngle, _maxAngle);
+    bool twistWasClamped = (twistAngle != clampedTwistAngle);
+
+    // update rotation
+    const float MIN_SWING_REAL_PART = 0.99999f;
+    if (twistWasClamped || fabsf(swingRotation.w < MIN_SWING_REAL_PART)) {
+        if (twistWasClamped) {
+            twistRotation = glm::angleAxis(clampedTwistAngle, _axis);
+        }
+        // we discard all swing and only keep twist
+        rotation = twistRotation * _referenceRotation;
+        return true;
+    }
+    return false;
+}
+

libraries/animation/src/ElbowConstraint.h

@@ -0,0 +1,30 @@
+//
+//  ElbowConstraint.h
+//
+//  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_ElbowConstraint_h
+#define hifi_ElbowConstraint_h
+
+#include "RotationConstraint.h"
+
+class ElbowConstraint : public RotationConstraint {
+public:
+    ElbowConstraint();
+    virtual void setReferenceRotation(const glm::quat& rotation) override { _referenceRotation = rotation; }
+    void setHingeAxis(const glm::vec3& axis);
+    void setAngleLimits(float minAngle, float maxAngle);
+    virtual bool apply(glm::quat& rotation) const override;
+protected:
+    glm::quat _referenceRotation;
+    glm::vec3 _axis;
+    glm::vec3 _perpAxis;
+    float _minAngle;
+    float _maxAngle;
+};
+
+#endif // hifi_ElbowConstraint_h

libraries/animation/src/RotationConstraint.h

@@ -0,0 +1,29 @@
+//
+//  RotationConstraint.h
+//
+//  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_RotationConstraint_h
+#define hifi_RotationConstraint_h
+
+#include <glm/glm.hpp>
+#include <glm/gtc/quaternion.hpp>
+
+class RotationConstraint {
+public:
+    RotationConstraint() {}
+    virtual ~RotationConstraint() {}
+
+    /// \param rotation the default rotation that represents 
+    virtual void setReferenceRotation(const glm::quat& rotation) = 0;
+
+    /// \param rotation rotation to clamp
+    /// \return true if rotation is clamped
+    virtual bool apply(glm::quat& rotation) const = 0;
+};
+
+#endif // hifi_RotationConstraint_h

libraries/animation/src/SwingTwistConstraint.cpp

@@ -0,0 +1,128 @@
+//
+//  SwingTwistConstraint.cpp
+//
+//  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 "SwingTwistConstraint.h"
+
+#include <algorithm>
+#include <math.h>
+
+#include <GeometryUtil.h>
+#include <NumericalConstants.h>
+
+
+const float MIN_MINDOT = -0.999f;
+const float MAX_MINDOT = 1.0f;
+
+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);
+}
+
+void SwingTwistConstraint::SwingLimitFunction::setMinDots(const std::vector<float>& minDots) {
+    int numDots = minDots.size();
+    _minDots.clear();
+    _minDots.reserve(numDots);
+    for (int i = 0; i < numDots; ++i) {
+        _minDots.push_back(glm::clamp(minDots[i], MIN_MINDOT, MAX_MINDOT));
+    }
+    // push the first value to the back to establish cyclic boundary conditions
+    _minDots.push_back(_minDots[0]);
+}
+
+float SwingTwistConstraint::SwingLimitFunction::getMinDot(float theta) const {
+    // extract the positive normalized fractional part of theta
+    float integerPart;
+    float normalizedTheta = modff(theta / TWO_PI, &integerPart);
+    if (normalizedTheta < 0.0f) {
+        normalizedTheta += 1.0f;
+    }
+
+    // interpolate between the two nearest points in the cycle
+    float fractionPart = modff(normalizedTheta * (float)(_minDots.size() - 1), &integerPart);
+    int i = (int)(integerPart);
+    int j = (i + 1) % _minDots.size();
+    return _minDots[i] * (1.0f - fractionPart) + _minDots[j] * fractionPart;
+}
+
+SwingTwistConstraint::SwingTwistConstraint() :
+        _swingLimitFunction(),
+        _referenceRotation(),
+        _minTwist(-PI),
+        _maxTwist(PI)
+{
+}
+
+void SwingTwistConstraint::setSwingLimits(std::vector<float> minDots) {
+    _swingLimitFunction.setMinDots(minDots);
+}
+
+void SwingTwistConstraint::setTwistLimits(float minTwist, float maxTwist) {
+    // NOTE: min/maxTwist angles should be in the range [-PI, PI]
+    _minTwist = glm::min(minTwist, maxTwist);
+    _maxTwist = glm::max(minTwist, maxTwist);
+}
+
+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);
+    // NOTE: postRotation = swingRotation * twistRotation
+
+    // compute 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));
+
+    // clamp twistAngle
+    float clampedTwistAngle = glm::clamp(twistAngle, _minTwist, _maxTwist);
+    bool twistWasClamped = (twistAngle != clampedTwistAngle);
+    
+    // clamp the swing
+    // The swingAxis is always perpendicular to the reference axis (yAxis in the constraint's frame).
+    glm::vec3 swungY = swingRotation * yAxis;
+    glm::vec3 swingAxis = glm::cross(yAxis, swungY);
+    bool swingWasClamped = false;
+    float axisLength = glm::length(swingAxis);
+    if (axisLength > EPSILON) {
+        // The limit of swing is a function of "theta" which can be computed from the swingAxis
+        // (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) {
+            // The swing limits are violated so we use the maxAngle to supply a new rotation.
+            float maxAngle = acosf(minDot);
+            if (minDot < 0.0f) {
+                maxAngle = PI - maxAngle;
+            }
+            swingAxis /= axisLength;
+            swingRotation = glm::angleAxis(maxAngle, swingAxis);
+            swingWasClamped = true;
+        }
+    }
+
+    if (swingWasClamped || twistWasClamped) {
+        twistRotation = glm::angleAxis(clampedTwistAngle, yAxis);
+        rotation = swingRotation * twistRotation * _referenceRotation;
+        return true;
+    }
+    return false;
+}
+

libraries/animation/src/SwingTwistConstraint.h

@@ -0,0 +1,79 @@
+//
+//  SwingTwistConstraint.h
+//
+//  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_SwingTwistConstraint_h
+#define hifi_SwingTwistConstraint_h
+
+#include <vector>
+
+#include "RotationConstraint.h"
+
+#include <math.h>
+
+class SwingTwistConstraint : RotationConstraint {
+public:
+    // The SwingTwistConstraint starts in the "referenceRotation" and then measures an initial twist 
+    // about the yAxis followed by a swing about some axis that lies in the XZ plane, such that the twist
+    // and swing combine to produce the rotation.  Each partial rotation is constrained within limits 
+    // then used to construct the new final rotation.
+
+    SwingTwistConstraint();
+
+    /// \param referenceRotation the rotation from which rotation changes are measured.
+    virtual void setReferenceRotation(const glm::quat& referenceRotation) override { _referenceRotation = referenceRotation; }
+
+    /// \param minDots vector of minimum dot products between the twist and swung axes.
+    /// \brief The values are minimum dot-products between the twist axis and the swung axes 
+    ///  that correspond to swing axes equally spaced around the XZ plane.  Another way to
+    ///  think about it is that the dot-products correspond to correspond to angles (theta) 
+    ///  about the twist axis ranging from 0 to 2PI-deltaTheta (Note: the cyclic boundary 
+    ///  conditions are handled internally, so don't duplicate the dot-product at 2PI).
+    ///  See the paper by Quang Liu and Edmond C. Prakash mentioned below for a more detailed 
+    ///  description of how this works.
+    void setSwingLimits(std::vector<float> minDots);
+
+    /// \param minTwist the minimum angle of rotation about the twist axis
+    /// \param maxTwist the maximum angle of rotation about the twist axis
+    void setTwistLimits(float minTwist, float maxTwist);
+
+    /// \param rotation[in/out] the current rotation to be modified to fit within constraints
+    /// \return true if rotation is changed
+    virtual bool apply(glm::quat& rotation) const override;
+
+    // SwingLimitFunction is an implementation of the constraint check described in the paper:
+    // "The Parameterization of Joint Rotation with the Unit Quaternion" by Quang Liu and Edmond C. Prakash
+    class SwingLimitFunction {
+    public:
+        SwingLimitFunction();
+    
+        /// \brief use a uniform conical swing limit
+        void setCone(float maxAngle);
+    
+        /// \brief use a vector of lookup values for swing limits
+        void setMinDots(const std::vector<float>& minDots);
+    
+        /// \return minimum dotProduct between reference and swung axes
+        float getMinDot(float theta) const;
+    
+    protected:
+        // the limits are stored in a lookup table with cyclic boundary conditions
+        std::vector<float> _minDots;
+    };
+
+    /// \return reference to SwingLimitFunction instance for unit-testing
+    const SwingLimitFunction& getSwingLimitFunction() const { return _swingLimitFunction; }
+
+protected:
+    SwingLimitFunction _swingLimitFunction;
+    glm::quat _referenceRotation;
+    float _minTwist;
+    float _maxTwist;
+};
+
+#endif // hifi_SwingTwistConstraint_h

libraries/shared/src/GeometryUtil.cpp

@@ -9,11 +9,13 @@
 //  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
 //
 
+#include "GeometryUtil.h"
+
+#include <assert.h>
 #include <cstring>
 #include <cmath>
 #include <glm/gtx/quaternion.hpp>
 
-#include "GeometryUtil.h"
 #include "NumericalConstants.h"
 
 glm::vec3 computeVectorFromPointToSegment(const glm::vec3& point, const glm::vec3& start, const glm::vec3& end) {
@@ -537,3 +539,21 @@ bool findRayRectangleIntersection(const glm::vec3& origin, const glm::vec3& dire
 
     return false;
 }
+
+void swingTwistDecomposition(const glm::quat& rotation,
+        const glm::vec3& direction,
+        glm::quat& swing,
+        glm::quat& twist) {
+    // direction MUST be normalized else the decomposition will be inaccurate
+    assert(fabsf(glm::length2(direction) - 1.0f) < 1.0e-4f); 
+
+    // the twist part has an axis (imaginary component) that is parallel to direction argument
+    glm::vec3 axisOfRotation(rotation.x, rotation.y, rotation.z);
+    glm::vec3 twistImaginaryPart = glm::dot(direction, axisOfRotation) * direction;
+    // and a real component that is relatively proportional to rotation's real component
+    twist = glm::normalize(glm::quat(rotation.w, twistImaginaryPart.x, twistImaginaryPart.y, twistImaginaryPart.z));
+
+    // once twist is known we can solve for swing:
+    // rotation = swing * twist  -->  swing = rotation * invTwist
+    swing = rotation * glm::inverse(twist);
+}

libraries/shared/src/GeometryUtil.h

@@ -82,6 +82,16 @@ bool findRayRectangleIntersection(const glm::vec3& origin, const glm::vec3& dire
 bool findRayTriangleIntersection(const glm::vec3& origin, const glm::vec3& direction,
                                     const glm::vec3& v0, const glm::vec3& v1, const glm::vec3& v2, float& distance);
 
+/// \brief decomposes rotation into its components such that: rotation = swing * twist
+/// \param rotation[in] rotation to decompose
+/// \param direction[in] normalized axis about which the twist happens (typically original direction before rotation applied)
+/// \param swing[out] the swing part of rotation
+/// \param twist[out] the twist part of rotation
+void swingTwistDecomposition(const glm::quat& rotation,
+        const glm::vec3& direction, // must be normalized
+        glm::quat& swing,
+        glm::quat& twist);
+
 class Triangle {
 public:
     glm::vec3 v0;

tests/QTestExtensions.h

@@ -168,7 +168,7 @@ bool QTest_compareWithAbsError(
     int line, const char* file,
     const V& epsilon
 ) {
-    if (abs(getErrorDifference(actual, expected)) > abs(epsilon)) {
+    if (fabsf(getErrorDifference(actual, expected)) > fabsf(epsilon)) {
         QTest_failWithMessage(
             "Compared values are not the same (fuzzy compare)",
             actual, expected, actual_expr, expected_expr, line, file,

tests/animation/src/RotationConstraintTests.cpp

@@ -0,0 +1,332 @@
+//
+//  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 (fabsf(expected) <= acceptableRelativeError) {
+            return fabsf(actual - expected) < fabsf(acceptableRelativeError);
+        }
+        return fabsf((actual - expected) / expected) < fabsf(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
+        const float ACCEPTABLE_ERROR = 1.0e-5f;
+        float theta = 0.0f;
+        float minDot = shoulderSwingLimitFunction.getMinDot(theta);
+        float expectedMinDot = lowDot;
+        QCOMPARE_WITH_RELATIVE_ERROR(minDot, expectedMinDot, ACCEPTABLE_ERROR);
+
+        theta = PI;
+        minDot = shoulderSwingLimitFunction.getMinDot(theta);
+        expectedMinDot = highDot;
+        QCOMPARE_WITH_RELATIVE_ERROR(minDot, expectedMinDot, ACCEPTABLE_ERROR);
+
+        // 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, ACCEPTABLE_ERROR);
+
+        // 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 = glm::clamp(twist, minTwistAngle, maxTwistAngle);
+
+            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 = glm::clamp(twist, minTwistAngle, maxTwistAngle);
+
+            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

tests/physics/src/ShapeColliderTests.h

@@ -1,53 +0,0 @@
-//
-//  ShapeColliderTests.h
-//  tests/physics/src
-//
-//  Created by Andrew Meadows on 02/21/2014.
-//  Copyright 2014 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_ShapeColliderTests_h
-#define hifi_ShapeColliderTests_h
-
-#include <QtTest/QtTest>
-
-class ShapeColliderTests : public QObject {
-    Q_OBJECT
-
-private slots:
-    void initTestCase();
-    
-    void sphereMissesSphere();
-    void sphereTouchesSphere();
-
-    void sphereMissesCapsule();
-    void sphereTouchesCapsule();
-
-    void capsuleMissesCapsule();
-    void capsuleTouchesCapsule();
-
-    void sphereTouchesAACubeFaces();
-    void sphereTouchesAACubeEdges();
-    void sphereTouchesAACubeCorners();
-    void sphereMissesAACube();
-
-    void capsuleMissesAACube();
-    void capsuleTouchesAACube();
-
-    void rayHitsSphere();
-    void rayBarelyHitsSphere();
-    void rayBarelyMissesSphere();
-    void rayHitsCapsule();
-    void rayMissesCapsule();
-    void rayHitsPlane();
-    void rayMissesPlane();
-    void rayHitsAACube();
-    void rayMissesAACube();
-
-    void measureTimeOfCollisionDispatch();
-};
-
-#endif // hifi_ShapeColliderTests_h

tests/shared/src/GeometryUtilTests.cpp

@@ -41,11 +41,9 @@ void GeometryUtilTests::testLocalRayRectangleIntersection() {
     glm::vec3 rectCenter(0.0f, 0.0f, 0.0f);
     glm::quat rectRotation = glm::quat(); // identity
 
-    // create points for generating rays that hit the plane and don't
+    { // verify hit
         glm::vec3 rayStart(1.0f, 2.0f, 3.0f);
         float delta = 0.1f;
-
-    { // verify hit
         glm::vec3 rayEnd = rectCenter + rectRotation * ((0.5f * rectDimensions.x - delta) * xAxis);
         glm::vec3 rayHitDirection = glm::normalize(rayEnd - rayStart);
         float expectedDistance = glm::length(rayEnd - rayStart);
@@ -57,6 +55,8 @@ void GeometryUtilTests::testLocalRayRectangleIntersection() {
     }
 
     { // verify miss
+        glm::vec3 rayStart(1.0f, 2.0f, 3.0f);
+        float delta = 0.1f;
         glm::vec3 rayEnd = rectCenter + rectRotation * ((0.5f * rectDimensions.y + delta) * yAxis);
         glm::vec3 rayMissDirection = glm::normalize(rayEnd - rayStart);
         float distance = FLT_MAX;
@@ -67,9 +67,9 @@ void GeometryUtilTests::testLocalRayRectangleIntersection() {
 
     { // hit with co-planer line
         float yFraction = 0.25f;
-        rayStart = rectCenter + rectRotation * (rectDimensions.x * xAxis + yFraction * rectDimensions.y * yAxis);
+        glm::vec3 rayStart = rectCenter + rectRotation * (rectDimensions.x * xAxis + yFraction * rectDimensions.y * yAxis);
 
-        glm::vec3 rayEnd = rectCenter - rectRotation * (rectDimensions.x * xAxis + yFraction * rectDimensions.y * yAxis);
+        glm::vec3 rayEnd = rectCenter - rectRotation * (rectDimensions.x * xAxis - yFraction * rectDimensions.y * yAxis);
         glm::vec3 rayHitDirection = glm::normalize(rayEnd - rayStart);
         float expectedDistance = rectDimensions.x;
 
@@ -81,9 +81,9 @@ void GeometryUtilTests::testLocalRayRectangleIntersection() {
 
     { // miss with co-planer line
         float yFraction = 0.75f;
-        rayStart = rectCenter + rectRotation * (rectDimensions.x * xAxis + (yFraction * rectDimensions.y) * yAxis);
+        glm::vec3 rayStart = rectCenter + rectRotation * (rectDimensions.x * xAxis + (yFraction * rectDimensions.y) * yAxis);
 
-        glm::vec3 rayEnd = rectCenter + rectRotation * (- rectDimensions.x * xAxis + (yFraction * rectDimensions.y) * yAxis);
+        glm::vec3 rayEnd = rectCenter - rectRotation * (rectDimensions.x * xAxis - (yFraction * rectDimensions.y) * yAxis);
         glm::vec3 rayHitDirection = glm::normalize(rayEnd - rayStart);
 
         float distance = FLT_MAX;
@@ -134,7 +134,7 @@ void GeometryUtilTests::testWorldRayRectangleIntersection() {
         float yFraction = 0.25f;
         rayStart = rectCenter + rectRotation * (rectDimensions.x * xAxis + (yFraction * rectDimensions.y) * yAxis);
 
-        glm::vec3 rayEnd = rectCenter - rectRotation * (rectDimensions.x * xAxis + (yFraction * rectDimensions.y) * yAxis);
+        glm::vec3 rayEnd = rectCenter - rectRotation * (rectDimensions.x * xAxis - (yFraction * rectDimensions.y) * yAxis);
         glm::vec3 rayHitDirection = glm::normalize(rayEnd - rayStart);
         float expectedDistance = rectDimensions.x;
 
@@ -148,7 +148,7 @@ void GeometryUtilTests::testWorldRayRectangleIntersection() {
         float yFraction = 0.75f;
         rayStart = rectCenter + rectRotation * (rectDimensions.x * xAxis + (yFraction * rectDimensions.y) * yAxis);
 
-        glm::vec3 rayEnd = rectCenter + rectRotation * (-rectDimensions.x * xAxis + (yFraction * rectDimensions.y) * yAxis);
+        glm::vec3 rayEnd = rectCenter - rectRotation * (rectDimensions.x * xAxis - (yFraction * rectDimensions.y) * yAxis);
         glm::vec3 rayHitDirection = glm::normalize(rayEnd - rayStart);
 
         float distance = FLT_MAX;
@@ -158,3 +158,64 @@ void GeometryUtilTests::testWorldRayRectangleIntersection() {
     }
 }
 
+void GeometryUtilTests::testTwistSwingDecomposition() {
+    // for each twist and swing angle pair:
+    // (a) compute twist and swing input components
+    // (b) compose the total rotation
+    // (c) decompose the total rotation
+    // (d) compare decomposed values with input components
+
+    glm::vec3 xAxis(1.0f, 0.0f, 0.0f);
+    glm::vec3 twistAxis = glm::normalize(glm::vec3(1.0f, 2.0f, 3.0f)); // can be anything but xAxis
+    glm::vec3 initialSwingAxis = glm::normalize(glm::cross(xAxis, twistAxis)); // initialSwingAxis must be perp to twistAxis
+
+    const int numTwists = 6;
+    const int numSwings = 7;
+    const int numSwingAxes = 5;
+
+    const float smallAngle = PI / 100.0f;
+
+    const float maxTwist = PI;
+    const float minTwist = -PI;
+    const float minSwing = 0.0f;
+    const float maxSwing = PI;
+
+    const float deltaTwist = (maxTwist - minTwist - 2.0f * smallAngle) / (float)(numTwists - 1);
+    const float deltaSwing = (maxSwing - minSwing - 2.0f * smallAngle) / (float)(numSwings - 1);
+
+    for (float twist = minTwist + smallAngle; twist < maxTwist; twist += deltaTwist) {
+        // compute twist component
+        glm::quat twistRotation = glm::angleAxis(twist, twistAxis);
+
+        float deltaTheta = TWO_PI / (numSwingAxes - 1);
+        for (float theta = 0.0f; theta < TWO_PI; theta += deltaTheta) {
+            // compute the swingAxis
+            glm::quat thetaRotation = glm::angleAxis(theta, twistAxis);
+            glm::vec3 swingAxis = thetaRotation * initialSwingAxis;
+
+            for (float swing = minSwing + smallAngle; swing < maxSwing; swing += deltaSwing) {
+                // compute swing component
+                glm::quat swingRotation = glm::angleAxis(swing, swingAxis);
+
+                // compose
+                glm::quat totalRotation = swingRotation * twistRotation;
+
+                // decompose
+                glm::quat measuredTwistRotation;
+                glm::quat measuredSwingRotation;
+                swingTwistDecomposition(totalRotation, twistAxis, measuredSwingRotation, measuredTwistRotation);
+    
+                // dot decomposed with components
+                float twistDot = fabsf(glm::dot(twistRotation, measuredTwistRotation));
+                float swingDot = fabsf(glm::dot(swingRotation, measuredSwingRotation));
+    
+                // the dot products should be very close to 1.0
+                const float MIN_ERROR = 1.0e-6f;
+                QCOMPARE_WITH_ABS_ERROR(1.0f, twistDot, MIN_ERROR);
+                QCOMPARE_WITH_ABS_ERROR(1.0f, swingDot, MIN_ERROR);
+            }
+        }
+    }
+}
+
+

tests/shared/src/GeometryUtilTests.h

@@ -20,6 +20,7 @@ class GeometryUtilTests : public QObject {
 private slots:
     void testLocalRayRectangleIntersection();
     void testWorldRayRectangleIntersection();
+    void testTwistSwingDecomposition();
 };
 
 float getErrorDifference(const float& a, const float& b);