add RotationConstraint and friends

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 = std::max(_minAngle, std::min(twistAngle, _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 @@
+//
+//  RotationConstrain.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 = std::max(MIN_MINDOT, std::min(cosf(maxAngle), 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(std::max(MIN_MINDOT, std::min(minDots[i], 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 = std::min(minTwist, maxTwist);
+    _maxTwist = std::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 = std::max(_minTwist, std::min(twistAngle, _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