fix some bugs in RotationConstraints

libraries/animation/src/ElbowConstraint.cpp

@@ -15,7 +15,6 @@
 #include <NumericalConstants.h>
 
 ElbowConstraint::ElbowConstraint() :
-        _referenceRotation(),
         _minAngle(-PI),
         _maxAngle(PI)
 {

libraries/animation/src/ElbowConstraint.h

@@ -15,12 +15,10 @@
 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;

libraries/animation/src/RotationConstraint.h

@@ -15,15 +15,21 @@
 
 class RotationConstraint {
 public:
-    RotationConstraint() {}
+    RotationConstraint() : _referenceRotation() {}
     virtual ~RotationConstraint() {}
 
-    /// \param rotation the default rotation that represents 
-    virtual void setReferenceRotation(const glm::quat& rotation) = 0;
+    /// \param referenceRotation the rotation from which rotation changes are measured.
+    virtual void setReferenceRotation(const glm::quat& rotation) { _referenceRotation = rotation; }
+
+    /// \return the rotation from which rotation changes are measured.
+    const glm::quat& getReferenceRotation() const { return _referenceRotation; }
 
     /// \param rotation rotation to clamp
     /// \return true if rotation is clamped
     virtual bool apply(glm::quat& rotation) const = 0;
+
+protected:
+    glm::quat _referenceRotation = glm::quat();
 };
 
 #endif // hifi_RotationConstraint_h

libraries/animation/src/SwingTwistConstraint.cpp

@@ -32,14 +32,20 @@ void SwingTwistConstraint::SwingLimitFunction::setCone(float maxAngle) {
 }
 
 void SwingTwistConstraint::SwingLimitFunction::setMinDots(const std::vector<float>& minDots) {
-    int numDots = minDots.size();
+    uint32_t 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));
+    if (numDots == 0) {
+        // push two copies of MIN_MINDOT
+        _minDots.push_back(MIN_MINDOT);
+        _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));
+        }
+        // push the first value to the back to establish cyclic boundary conditions
+        _minDots.push_back(_minDots[0]);
     }
-    // push the first value to the back to establish cyclic boundary conditions
-    _minDots.push_back(_minDots[0]);
 }
 
 float SwingTwistConstraint::SwingLimitFunction::getMinDot(float theta) const {
@@ -58,8 +64,8 @@ float SwingTwistConstraint::SwingLimitFunction::getMinDot(float theta) const {
 }
 
 SwingTwistConstraint::SwingTwistConstraint() :
+        RotationConstraint(),
         _swingLimitFunction(),
-        _referenceRotation(),
         _minTwist(-PI),
         _maxTwist(PI)
 {
@@ -69,6 +75,85 @@ void SwingTwistConstraint::setSwingLimits(std::vector<float> minDots) {
     _swingLimitFunction.setMinDots(minDots);
 }
 
+void SwingTwistConstraint::setSwingLimits(const std::vector<glm::vec3>& swungDirections) {
+    struct SwingLimitData {
+        SwingLimitData() : _theta(0.0f), _minDot(1.0f) {}
+        SwingLimitData(float theta, float minDot) : _theta(theta), _minDot(minDot) {}
+        float _theta;
+        float _minDot;
+        bool operator<(const SwingLimitData& other) const { return _theta < other._theta; }
+    };
+    std::vector<SwingLimitData> limits;
+
+    uint32_t numLimits = swungDirections.size();
+    limits.reserve(numLimits);
+
+    // compute the limit pairs: <theta, minDot>
+    const glm::vec3 yAxis = glm::vec3(0.0f, 1.0f, 0.0f);
+    for (uint32_t i = 0; i < numLimits; ++i) {
+        float directionLength = glm::length(swungDirections[i]);
+        if (directionLength > EPSILON) {
+            glm::vec3 swingAxis = glm::cross(yAxis, swungDirections[i]);
+            float theta = atan2f(-swingAxis.z, swingAxis.x);
+            if (theta < 0.0f) {
+                theta += TWO_PI;
+            }
+            limits.push_back(SwingLimitData(theta, swungDirections[i].y / directionLength));
+        }
+    }
+
+    std::vector<float> minDots;
+    numLimits = limits.size();
+    if (numLimits == 0) {
+        // trivial case: nearly free constraint
+        std::vector<float> minDots;
+        _swingLimitFunction.setMinDots(minDots);
+    } else if (numLimits == 1) {
+        // trivial case: uniform conical constraint
+        std::vector<float> minDots;
+        minDots.push_back(limits[0]._minDot);
+        _swingLimitFunction.setMinDots(minDots);
+    } else {
+        // interesting case: potentially non-uniform constraints
+
+        // sort limits by theta
+        std::sort(limits.begin(), limits.end());
+    
+        // 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) {
+            float theta = (float)i * deltaTheta;
+            uint32_t leftIndex = (rightIndex - 1) % numLimits;
+            while (rightIndex < numLimits && theta > limits[rightIndex]._theta) {
+                leftIndex = rightIndex++;
+            }
+
+            if (leftIndex == numLimits - 1) {
+                // we straddle the boundary
+                rightIndex = 0;
+            }
+
+            float rightTheta = limits[rightIndex]._theta;
+            float leftTheta = limits[leftIndex]._theta;
+            if (leftTheta > rightTheta) {
+                // we straddle the boundary, but we need to figure out which way to stride
+                // in order to keep theta between left and right
+                if (leftTheta > theta) {
+                    leftTheta -= TWO_PI;
+                } else {
+                    rightTheta += TWO_PI;
+                }
+            }
+
+            // blend between the left and right minDots to get the value that corresponds to this theta
+            float rightWeight = (theta - leftTheta) / (rightTheta - leftTheta);
+            minDots.push_back((1.0f - rightWeight) * limits[leftIndex]._minDot + rightWeight * limits[rightIndex]._minDot);
+        }
+    }
+    _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);
@@ -107,13 +192,10 @@ bool SwingTwistConstraint::apply(glm::quat& rotation) const {
         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;
-            }
+            // The swing limits are violated so we extract the angle from midDot and 
+            // use it to supply a new rotation.
             swingAxis /= axisLength;
-            swingRotation = glm::angleAxis(maxAngle, swingAxis);
+            swingRotation = glm::angleAxis(acosf(minDot), swingAxis);
             swingWasClamped = true;
         }
     }

libraries/animation/src/SwingTwistConstraint.h

@@ -16,7 +16,7 @@
 
 #include <math.h>
 
-class SwingTwistConstraint : RotationConstraint {
+class SwingTwistConstraint : public 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
@@ -25,10 +25,7 @@ public:
 
     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.
+    /// \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) 
@@ -38,6 +35,13 @@ public:
     ///  description of how this works.
     void setSwingLimits(std::vector<float> minDots);
 
+    /// \param swungDirections vector of directions that lie on the swing limit boundary
+    /// \brief For each swungDirection we compute the corresponding [theta, minDot] pair.
+    /// We expect the values of theta to NOT be uniformly spaced around the range [0, TWO_PI]
+    /// so we'll use the input set to extrapolate a lookup function of evenly spaced values.
+    void setSwingLimits(const std::vector<glm::vec3>& swungDirections);
+
+
     /// \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);
@@ -71,7 +75,6 @@ public:
 
 protected:
     SwingLimitFunction _swingLimitFunction;
-    glm::quat _referenceRotation;
     float _minTwist;
     float _maxTwist;
 };