SwingTwistConstraint.h

//
//  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 : 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
    // and swing combine to produce the rotation.  Each partial rotation is constrained within limits
    // then used to construct the new final rotation.
 
    SwingTwistConstraint();
 
    void setSwingLimits(std::vector<float> minDots);
 
    void setSwingLimits(const std::vector<glm::vec3>& swungDirections);
 
 
    void setTwistLimits(float minTwist, float maxTwist);
 
    virtual bool apply(glm::quat& rotation) const override;
 
    void setLowerSpine(bool lowerSpine) { _lowerSpine = lowerSpine; }
    virtual bool isLowerSpine() const override { return _lowerSpine; }
 
    virtual void dynamicallyAdjustLimits(const glm::quat& rotation) override;
 
    // for testing purposes
    const std::vector<float>& getMinDots() const { 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();
 
        void setMinDots(const std::vector<float>& minDots);
 
        void dynamicallyAdjustMinDots(float theta, float minDot);
 
        float getMinDot(float theta) const;
 
        // for testing purposes
        const std::vector<float>& getMinDots() const { return _minDots; }
 
    private:
        // the limits are stored in a lookup table with cyclic boundary conditions
        std::vector<float> _minDots;
 
        // these values used to restore dynamic adjustment
        float _minDotA;
        float _minDotB;
        int8_t _minDotIndexA;
        int8_t _minDotIndexB;
    };
 
    const SwingLimitFunction& getSwingLimitFunction() const { return _swingLimitFunction; }
 
    void clearHistory() override;
 
    virtual glm::quat computeCenterRotation() const override;
 
    float getMinTwist() const { return _minTwist; }
    float getMaxTwist() const { return _maxTwist; }
 
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