diff --git a/libraries/animation/src/ElbowConstraint.cpp b/libraries/animation/src/ElbowConstraint.cpp new file mode 100644 index 0000000000..e575b3f8b7 --- /dev/null +++ b/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 + +#include +#include + +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; +} + diff --git a/libraries/animation/src/ElbowConstraint.h b/libraries/animation/src/ElbowConstraint.h new file mode 100644 index 0000000000..3c4481cfa8 --- /dev/null +++ b/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 diff --git a/libraries/animation/src/RotationConstraint.h b/libraries/animation/src/RotationConstraint.h new file mode 100644 index 0000000000..e664a9d8af --- /dev/null +++ b/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 +#include + +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 diff --git a/libraries/animation/src/SwingTwistConstraint.cpp b/libraries/animation/src/SwingTwistConstraint.cpp new file mode 100644 index 0000000000..9c88b31637 --- /dev/null +++ b/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 +#include + +#include +#include + + +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& 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 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; +} + diff --git a/libraries/animation/src/SwingTwistConstraint.h b/libraries/animation/src/SwingTwistConstraint.h new file mode 100644 index 0000000000..c5e6d9e569 --- /dev/null +++ b/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 + +#include "RotationConstraint.h" + +#include + +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 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& 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 _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