463 lines · 17.5 KB · C++
RawDownload
////  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 <GLMHelpers.h>#include <NumericalConstants.h>#include "AnimUtil.h"  const float MIN_MINDOT = -0.999f;const float MAX_MINDOT = 1.0f; const int LAST_CLAMP_LOW_BOUNDARY = -1;const int LAST_CLAMP_NO_BOUNDARY = 0;const int LAST_CLAMP_HIGH_BOUNDARY = 1; SwingTwistConstraint::SwingLimitFunction::SwingLimitFunction() {    _minDots.push_back(-1.0f);    _minDots.push_back(-1.0f);     _minDotIndexA = -1;    _minDotIndexB = -1;} // In order to support the dynamic adjustment to swing limits we require// that minDots have a minimum number of elements:const int MIN_NUM_DOTS = 8; void SwingTwistConstraint::SwingLimitFunction::setMinDots(const std::vector<float>& minDots) {    int numDots = (int)minDots.size();    _minDots.clear();    if (numDots == 0) {        // push multiple copies of MIN_MINDOT        for (int i = 0; i < MIN_NUM_DOTS; ++i) {            _minDots.push_back(MIN_MINDOT);        }        // push one more for cyclic boundary conditions        _minDots.push_back(MIN_MINDOT);    } else {        // for minimal fidelity in the dynamic adjustment we expand the swing limit data until        // we have enough data points        int trueNumDots = numDots;        int numFiller = 0;        while(trueNumDots < MIN_NUM_DOTS) {            numFiller++;            trueNumDots += numDots;        }        _minDots.reserve(trueNumDots);         for (int i = 0; i < numDots; ++i) {            // push the next value            _minDots.push_back(glm::clamp(minDots[i], MIN_MINDOT, MAX_MINDOT));             if (numFiller > 0) {                // compute endpoints of line segment                float nearDot = glm::clamp(minDots[i], MIN_MINDOT, MAX_MINDOT);                int k = (i + 1) % numDots;                float farDot = glm::clamp(minDots[k], MIN_MINDOT, MAX_MINDOT);                 // fill the gap with interpolated values                for (int j = 0; j < numFiller; ++j) {                    float delta = (float)(j + 1) / float(numFiller + 1);                    _minDots.push_back((1.0f - delta) * nearDot + delta * farDot);                }            }        }        // push the first value to the back to for cyclic boundary conditions        _minDots.push_back(_minDots[0]);    }    _minDotIndexA = -1;    _minDotIndexB = -1;} /// \param angle radian angle to update/// \param minDotAdjustment minimum dot limit at that anglevoid SwingTwistConstraint::SwingLimitFunction::dynamicallyAdjustMinDots(float theta, float minDotAdjustment) {    // What does "dynamic adjustment" mean?    //    // Consider a limitFunction that looks like this:    //    //  1+    //   |                valid space    //   |    //   +-----+-----+-----+-----+-----+-----+-----+-----+    //   |    //   |                invalid space    //  0+------------------------------------------------    //   0          pi/2         pi         3pi/2       2pi    //   theta --->    //    // If we wanted to modify the envelope to accept a single invalid point X    // then we would need to modify neighboring values A and B accordingly:    //    //  1+          adjustment for X at some thetaX    //   |              |    //   |              |    //   +-----+.       V       .+-----+-----+-----+-----+    //   |        -           -    //   |         ' A--X--B '    //  0+------------------------------------------------    //   0          pi/2         pi         3pi/2       2pi    //    // The code below computes the values of A and B such that the line between them    // passes through the point X, and we get reasonable interpolation for nearby values    // of theta.  The old AB values are saved for later restore.     if (_minDotIndexA > -1) {        // retstore old values        _minDots[_minDotIndexA] = _minDotA;        _minDots[_minDotIndexB] = _minDotB;         // handle cyclic boundary conditions        int lastIndex = (int)_minDots.size() - 1;        if (_minDotIndexA == 0) {            _minDots[lastIndex] = _minDotA;        } else if (_minDotIndexB == lastIndex) {            _minDots[0] = _minDotB;        }    }     // extract the positive normalized fractional part of the theta    float integerPart;    float normalizedAngle = modff(theta / TWO_PI, &integerPart);    if (normalizedAngle < 0.0f) {        normalizedAngle += 1.0f;    }     // interpolate between the two nearest points in the curve    float delta = modff(normalizedAngle * (float)(_minDots.size() - 1), &integerPart);    int indexA = (int)(integerPart);    int indexB = (indexA + 1) % _minDots.size();    float interpolatedDot = _minDots[indexA] * (1.0f - delta) + _minDots[indexB] * delta;     if (minDotAdjustment < interpolatedDot) {        // minDotAdjustment is outside the existing bounds so we must modify         // remember the indices        _minDotIndexA = indexA;        _minDotIndexB = indexB;         // save the old minDots        _minDotA = _minDots[_minDotIndexA];        _minDotB = _minDots[_minDotIndexB];         // compute replacement values to _minDots that will provide a line segment        // that passes through minDotAdjustment while balancing the distortion between A and B.        // Note: the derivation of these formulae is left as an exercise to the reader.        float twiceUndershoot = 2.0f * (minDotAdjustment - interpolatedDot);        _minDots[_minDotIndexA] -= twiceUndershoot * (delta + 0.5f) * (delta - 1.0f);        _minDots[_minDotIndexB] -= twiceUndershoot * delta * (delta - 1.5f);         // handle cyclic boundary conditions        int lastIndex = (int)_minDots.size() - 1;        if (_minDotIndexA == 0) {            _minDots[lastIndex] = _minDots[_minDotIndexA];        } else if (_minDotIndexB == lastIndex) {            _minDots[0] = _minDots[_minDotIndexB];        }    } else {        // minDotAdjustment is inside bounds so there is nothing to do        _minDotIndexA = -1;        _minDotIndexB = -1;    }} 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() :        RotationConstraint(),        _swingLimitFunction(),        _minTwist(-PI),        _maxTwist(PI),        _lastTwistBoundary(LAST_CLAMP_NO_BOUNDARY){} 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;     int numLimits = (int)swungDirections.size();    limits.reserve(numLimits);     // compute the limit pairs: <theta, minDot>    for (int i = 0; i < numLimits; ++i) {        float directionLength = glm::length(swungDirections[i]);        if (directionLength > EPSILON) {            glm::vec3 swingAxis = glm::cross(Vectors::UNIT_Y, 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 = (int)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);        int rightIndex = 0;        for (int i = 0; i < numLimits; ++i) {            float theta = (float)i * deltaTheta;            int leftIndex = (rightIndex - 1 + numLimits) % 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);    _maxTwist = glm::max(minTwist, maxTwist);     _lastTwistBoundary = LAST_CLAMP_NO_BOUNDARY;    _twistAdjusted = false;} // privatefloat SwingTwistConstraint::handleTwistBoundaryConditions(float twistAngle) const {    // adjust measured twistAngle according to clamping history    switch (_lastTwistBoundary) {        case LAST_CLAMP_LOW_BOUNDARY:            // clamp to min            if (twistAngle > _maxTwist) {                twistAngle -= TWO_PI;            }            break;        case LAST_CLAMP_HIGH_BOUNDARY:            // clamp to max            if (twistAngle < _minTwist) {                twistAngle += TWO_PI;            }            break;        default: // LAST_CLAMP_NO_BOUNDARY            // clamp to nearest boundary            float midBoundary = 0.5f * (_maxTwist + _minTwist + TWO_PI);            if (twistAngle > midBoundary) {                // lower boundary is closer --> phase down one cycle                twistAngle -= TWO_PI;            } else if (twistAngle < midBoundary - TWO_PI) {                // higher boundary is closer --> phase up one cycle                twistAngle += TWO_PI;            }            break;    }    return twistAngle;} bool SwingTwistConstraint::apply(glm::quat& rotation) const {    // decompose the rotation into first twist about yAxis, then swing about something perp    // NOTE: rotation = postRotation * referenceRotation    glm::quat postRotation = rotation * glm::inverse(_referenceRotation);    glm::quat swingRotation, twistRotation;    swingTwistDecomposition(postRotation, Vectors::UNIT_Y, swingRotation, twistRotation);    // NOTE: postRotation = swingRotation * twistRotation     // compute raw twistAngle    float twistAngle = 2.0f * acosf(fabsf(twistRotation.w));    glm::vec3 twistedX = twistRotation * Vectors::UNIT_X;    twistAngle *= copysignf(1.0f, glm::dot(glm::cross(Vectors::UNIT_X, twistedX), Vectors::UNIT_Y));     bool somethingClamped = false;    if (_minTwist != _maxTwist) {        // twist limits apply --> figure out which limit we're hitting, if any        twistAngle = handleTwistBoundaryConditions(twistAngle);         // clamp twistAngle        float clampedTwistAngle = glm::clamp(twistAngle, _minTwist, _maxTwist);        somethingClamped = (twistAngle != clampedTwistAngle);         // remember twist's clamp boundary history        if (somethingClamped) {            _lastTwistBoundary = (twistAngle > clampedTwistAngle) ? LAST_CLAMP_HIGH_BOUNDARY : LAST_CLAMP_LOW_BOUNDARY;            twistAngle = clampedTwistAngle;        } else {            _lastTwistBoundary = LAST_CLAMP_NO_BOUNDARY;        }    }     // clamp the swing    // The swingAxis is always perpendicular to the reference axis (yAxis in the constraint's frame).    glm::vec3 swungY = swingRotation * Vectors::UNIT_Y;    glm::vec3 swingAxis = glm::cross(Vectors::UNIT_Y, swungY);    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, Vectors::UNIT_Y) < minDot) {            // 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(acosf(minDot), swingAxis);            somethingClamped = true;        }    }     if (somethingClamped) {        // update the rotation        twistRotation = glm::angleAxis(twistAngle, Vectors::UNIT_Y);        rotation = swingRotation * twistRotation * _referenceRotation;        return true;    }    return false;} void SwingTwistConstraint::dynamicallyAdjustLimits(const glm::quat& rotation) {    glm::quat postRotation = rotation * glm::inverse(_referenceRotation);    glm::quat swingRotation, twistRotation;     swingTwistDecomposition(postRotation, Vectors::UNIT_Y, swingRotation, twistRotation);     { // adjust swing limits        glm::vec3 swungY = swingRotation * Vectors::UNIT_Y;        glm::vec3 swingAxis = glm::cross(Vectors::UNIT_Y, swungY);        float theta = atan2f(-swingAxis.z, swingAxis.x);        if (glm::isnan(theta)) {            // atan2f() will only return NaN if either of its arguments is NaN, which can only            // happen if we've been given a bad rotation.  Since a NaN value here could potentially            // cause a crash (we use the value of theta to compute indices into a std::vector)            // we specifically check for this case.            theta = 0.0f;            swungY.y = 1.0f;        }        _swingLimitFunction.dynamicallyAdjustMinDots(theta, swungY.y);    }     // restore twist limits    if (_twistAdjusted) {        _minTwist = _oldMinTwist;        _maxTwist = _oldMaxTwist;        _twistAdjusted = false;    }     if (_minTwist != _maxTwist) {        // compute twistAngle        float twistAngle = 2.0f * acosf(fabsf(twistRotation.w));        glm::vec3 twistedX = twistRotation * Vectors::UNIT_X;        twistAngle *= copysignf(1.0f, glm::dot(glm::cross(Vectors::UNIT_X, twistedX), Vectors::UNIT_Y));        twistAngle = handleTwistBoundaryConditions(twistAngle);         if (twistAngle < _minTwist || twistAngle > _maxTwist) {            // expand twist limits            _twistAdjusted = true;            _oldMinTwist = _minTwist;            _oldMaxTwist = _maxTwist;            if (twistAngle < _minTwist) {                _minTwist = twistAngle;            } else if (twistAngle > _maxTwist) {                _maxTwist = twistAngle;            }        }    }} void SwingTwistConstraint::clearHistory() {    _lastTwistBoundary = LAST_CLAMP_NO_BOUNDARY;} glm::quat SwingTwistConstraint::computeCenterRotation() const {    const size_t NUM_TWIST_LIMITS = 2;    const size_t NUM_MIN_DOTS = getMinDots().size();    std::vector<glm::quat> swingLimits;    swingLimits.reserve(NUM_MIN_DOTS);     glm::quat twistLimits[NUM_TWIST_LIMITS];    if (_minTwist != _maxTwist) {        // to ensure that twists do not flip the center rotation, we devide twist angle by 2.        twistLimits[0] = glm::angleAxis(_minTwist / 2.0f, _referenceRotation * Vectors::UNIT_Y);        twistLimits[1] = glm::angleAxis(_maxTwist / 2.0f, _referenceRotation * Vectors::UNIT_Y);    }    const float D_THETA = TWO_PI / (NUM_MIN_DOTS - 1);    float theta = 0.0f;    for (size_t i = 0; i < NUM_MIN_DOTS - 1; i++, theta += D_THETA) {        // compute swing rotation from theta and phi angles.        float phi = acos(getMinDots()[i]);        float cos_phi = getMinDots()[i];        float sin_phi = sinf(phi);        glm::vec3 swungAxis(sin_phi * cosf(theta), cos_phi, -sin_phi * sinf(theta));         // to ensure that swings > 90 degrees do not flip the center rotation, we devide phi / 2        glm::quat swing = glm::angleAxis(phi / 2, glm::normalize(glm::cross(Vectors::UNIT_Y, swungAxis)));        swingLimits.push_back(swing);    }    glm::quat averageSwing = averageQuats(swingLimits.size(), &swingLimits[0]);    glm::quat averageTwist = averageQuats(2, twistLimits);    return averageSwing * averageTwist * _referenceRotation;}

Keyboard shortcuts

General

Show keyboard shortcuts
?
Focus the current search
/

Issues and pull requests

Open filters
f

Issues, pull requests and Inbox

Select next item
j↓
Select previous item
k↑
Open selected item
Enter
Dismiss filters or clear selection
Esc

Repository code

Go to file
t