mirror of
https://github.com/overte-org/overte.git
synced 2025-04-20 14:03:55 +02:00
Merge pull request #7328 from AndrewMeadows/dynamic-constraints
reduce clamping of animation underpose by IK system
This commit is contained in:
Anthony Thibault
commit
5623fc1b89
8 changed files with 730 additions and 145 deletions
|
|
@ -23,6 +23,8 @@ AnimInverseKinematics::AnimInverseKinematics(const QString& id) : AnimNode(AnimN
|
|||
|
||||
AnimInverseKinematics::~AnimInverseKinematics() {
|
||||
clearConstraints();
|
||||
_accumulators.clear();
|
||||
_targetVarVec.clear();
|
||||
}
|
||||
|
||||
void AnimInverseKinematics::loadDefaultPoses(const AnimPoseVec& poses) {
|
||||
|
|
@ -394,6 +396,17 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
|
|||
}
|
||||
_relativePoses[i].trans = underPoses[i].trans;
|
||||
}
|
||||
|
||||
if (!_relativePoses.empty()) {
|
||||
// Sometimes the underpose itself can violate the constraints. Rather than
|
||||
// clamp the animation we dynamically expand each constraint to accomodate it.
|
||||
std::map<int, RotationConstraint*>::iterator constraintItr = _constraints.begin();
|
||||
while (constraintItr != _constraints.end()) {
|
||||
int index = constraintItr->first;
|
||||
constraintItr->second->dynamicallyAdjustLimits(_relativePoses[index].rot);
|
||||
++constraintItr;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!_relativePoses.empty()) {
|
||||
|
|
|
|||
|
|
@ -31,6 +31,10 @@ public:
|
|||
/// \return true if this constraint is part of lower spine
|
||||
virtual bool isLowerSpine() const { return false; }
|
||||
|
||||
/// \param rotation rotation to allow
|
||||
/// \brief clear previous adjustment and adjust constraint limits to allow rotation
|
||||
virtual void dynamicallyAdjustLimits(const glm::quat& rotation) {}
|
||||
|
||||
protected:
|
||||
glm::quat _referenceRotation = glm::quat();
|
||||
};
|
||||
|
|
|
|||
|
|
@ -13,6 +13,7 @@
|
|||
#include <math.h>
|
||||
|
||||
#include <GeometryUtil.h>
|
||||
#include <GLMHelpers.h>
|
||||
#include <NumericalConstants.h>
|
||||
|
||||
|
||||
|
|
@ -24,32 +25,152 @@ const int LAST_CLAMP_NO_BOUNDARY = 0;
|
|||
const int LAST_CLAMP_HIGH_BOUNDARY = 1;
|
||||
|
||||
SwingTwistConstraint::SwingLimitFunction::SwingLimitFunction() {
|
||||
setCone(PI);
|
||||
_minDots.push_back(-1.0f);
|
||||
_minDots.push_back(-1.0f);
|
||||
|
||||
_minDotIndexA = -1;
|
||||
_minDotIndexB = -1;
|
||||
}
|
||||
|
||||
void SwingTwistConstraint::SwingLimitFunction::setCone(float maxAngle) {
|
||||
_minDots.clear();
|
||||
float minDot = glm::clamp(maxAngle, MIN_MINDOT, MAX_MINDOT);
|
||||
_minDots.push_back(minDot);
|
||||
// push the first value to the back to establish cyclic boundary conditions
|
||||
_minDots.push_back(minDot);
|
||||
}
|
||||
// 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) {
|
||||
uint32_t numDots = (uint32_t)minDots.size();
|
||||
int numDots = (int)minDots.size();
|
||||
_minDots.clear();
|
||||
if (numDots == 0) {
|
||||
// push two copies of MIN_MINDOT
|
||||
_minDots.push_back(MIN_MINDOT);
|
||||
// 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 {
|
||||
_minDots.reserve(numDots);
|
||||
for (uint32_t i = 0; i < numDots; ++i) {
|
||||
_minDots.push_back(glm::clamp(minDots[i], MIN_MINDOT, MAX_MINDOT));
|
||||
// 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;
|
||||
}
|
||||
// push the first value to the back to establish cyclic boundary conditions
|
||||
_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 angle
|
||||
void 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 {
|
||||
|
|
@ -90,15 +211,14 @@ void SwingTwistConstraint::setSwingLimits(const std::vector<glm::vec3>& swungDir
|
|||
};
|
||||
std::vector<SwingLimitData> limits;
|
||||
|
||||
uint32_t numLimits = (uint32_t)swungDirections.size();
|
||||
int numLimits = (int)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) {
|
||||
for (int i = 0; i < numLimits; ++i) {
|
||||
float directionLength = glm::length(swungDirections[i]);
|
||||
if (directionLength > EPSILON) {
|
||||
glm::vec3 swingAxis = glm::cross(yAxis, swungDirections[i]);
|
||||
glm::vec3 swingAxis = glm::cross(Vectors::UNIT_Y, swungDirections[i]);
|
||||
float theta = atan2f(-swingAxis.z, swingAxis.x);
|
||||
if (theta < 0.0f) {
|
||||
theta += TWO_PI;
|
||||
|
|
@ -108,7 +228,7 @@ void SwingTwistConstraint::setSwingLimits(const std::vector<glm::vec3>& swungDir
|
|||
}
|
||||
|
||||
std::vector<float> minDots;
|
||||
numLimits = (uint32_t)limits.size();
|
||||
numLimits = (int)limits.size();
|
||||
if (numLimits == 0) {
|
||||
// trivial case: nearly free constraint
|
||||
std::vector<float> minDots;
|
||||
|
|
@ -126,10 +246,10 @@ void SwingTwistConstraint::setSwingLimits(const std::vector<glm::vec3>& swungDir
|
|||
|
||||
// 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) {
|
||||
int rightIndex = 0;
|
||||
for (int i = 0; i < numLimits; ++i) {
|
||||
float theta = (float)i * deltaTheta;
|
||||
uint32_t leftIndex = (rightIndex - 1) % numLimits;
|
||||
int leftIndex = (rightIndex - 1) % numLimits;
|
||||
while (rightIndex < numLimits && theta > limits[rightIndex]._theta) {
|
||||
leftIndex = rightIndex++;
|
||||
}
|
||||
|
|
@ -165,51 +285,57 @@ void SwingTwistConstraint::setTwistLimits(float minTwist, float maxTwist) {
|
|||
_maxTwist = glm::max(minTwist, maxTwist);
|
||||
|
||||
_lastTwistBoundary = LAST_CLAMP_NO_BOUNDARY;
|
||||
_twistAdjusted = false;
|
||||
}
|
||||
|
||||
// private
|
||||
float 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
|
||||
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);
|
||||
swingTwistDecomposition(postRotation, Vectors::UNIT_Y, swingRotation, twistRotation);
|
||||
// NOTE: postRotation = swingRotation * twistRotation
|
||||
|
||||
// compute twistAngle
|
||||
// compute raw 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));
|
||||
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) {
|
||||
// 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;
|
||||
}
|
||||
// twist limits apply --> figure out which limit we're hitting, if any
|
||||
twistAngle = handleTwistBoundaryConditions(twistAngle);
|
||||
|
||||
// clamp twistAngle
|
||||
float clampedTwistAngle = glm::clamp(twistAngle, _minTwist, _maxTwist);
|
||||
|
|
@ -226,15 +352,15 @@ bool SwingTwistConstraint::apply(glm::quat& rotation) const {
|
|||
|
||||
// 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);
|
||||
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, yAxis) < minDot) {
|
||||
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;
|
||||
|
|
@ -245,13 +371,53 @@ bool SwingTwistConstraint::apply(glm::quat& rotation) const {
|
|||
|
||||
if (somethingClamped) {
|
||||
// update the rotation
|
||||
twistRotation = glm::angleAxis(twistAngle, yAxis);
|
||||
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);
|
||||
_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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -53,24 +53,45 @@ public:
|
|||
void setLowerSpine(bool lowerSpine) { _lowerSpine = lowerSpine; }
|
||||
virtual bool isLowerSpine() const override { return _lowerSpine; }
|
||||
|
||||
/// \param rotation rotation to allow
|
||||
/// \brief clear previous adjustment and adjust constraint limits to allow rotation
|
||||
virtual void dynamicallyAdjustLimits(const glm::quat& rotation) override;
|
||||
|
||||
// for testing purposes
|
||||
const std::vector<float>& getMinDots() { 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();
|
||||
|
||||
/// \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);
|
||||
|
||||
/// \param theta radian angle to new minDot
|
||||
/// \param minDot minimum dot limit
|
||||
/// \brief updates swing constraint to permit minDot at theta
|
||||
void dynamicallyAdjustMinDots(float theta, float minDot);
|
||||
|
||||
/// \return minimum dotProduct between reference and swung axes
|
||||
float getMinDot(float theta) const;
|
||||
|
||||
protected:
|
||||
// for testing purposes
|
||||
const std::vector<float>& getMinDots() { 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;
|
||||
};
|
||||
|
||||
/// \return reference to SwingLimitFunction instance for unit-testing
|
||||
|
|
@ -79,15 +100,22 @@ public:
|
|||
/// \brief exposed for unit testing
|
||||
void clearHistory();
|
||||
|
||||
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
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@ const glm::quat identity = glm::quat();
|
|||
const glm::quat quaterTurnAroundZ = glm::angleAxis(0.5f * PI, zAxis);
|
||||
|
||||
|
||||
void makeTestFBXJoints(std::vector<FBXJoint>& fbxJoints) {
|
||||
void makeTestFBXJoints(FBXGeometry& geometry) {
|
||||
FBXJoint joint;
|
||||
joint.isFree = false;
|
||||
joint.freeLineage.clear();
|
||||
|
|
@ -61,29 +61,29 @@ void makeTestFBXJoints(std::vector<FBXJoint>& fbxJoints) {
|
|||
joint.name = "A";
|
||||
joint.parentIndex = -1;
|
||||
joint.translation = origin;
|
||||
fbxJoints.push_back(joint);
|
||||
geometry.joints.push_back(joint);
|
||||
|
||||
joint.name = "B";
|
||||
joint.parentIndex = 0;
|
||||
joint.translation = xAxis;
|
||||
fbxJoints.push_back(joint);
|
||||
geometry.joints.push_back(joint);
|
||||
|
||||
joint.name = "C";
|
||||
joint.parentIndex = 1;
|
||||
joint.translation = xAxis;
|
||||
fbxJoints.push_back(joint);
|
||||
geometry.joints.push_back(joint);
|
||||
|
||||
joint.name = "D";
|
||||
joint.parentIndex = 2;
|
||||
joint.translation = xAxis;
|
||||
fbxJoints.push_back(joint);
|
||||
geometry.joints.push_back(joint);
|
||||
|
||||
// compute each joint's transform
|
||||
for (int i = 1; i < (int)fbxJoints.size(); ++i) {
|
||||
FBXJoint& j = fbxJoints[i];
|
||||
for (int i = 1; i < (int)geometry.joints.size(); ++i) {
|
||||
FBXJoint& j = geometry.joints[i];
|
||||
int parentIndex = j.parentIndex;
|
||||
// World = ParentWorld * T * (Roff * Rp) * Rpre * R * Rpost * (Rp-1 * Soff * Sp * S * Sp-1)
|
||||
j.transform = fbxJoints[parentIndex].transform *
|
||||
j.transform = geometry.joints[parentIndex].transform *
|
||||
glm::translate(j.translation) *
|
||||
j.preTransform *
|
||||
glm::mat4_cast(j.preRotation * j.rotation * j.postRotation) *
|
||||
|
|
@ -94,14 +94,14 @@ void makeTestFBXJoints(std::vector<FBXJoint>& fbxJoints) {
|
|||
}
|
||||
|
||||
void AnimInverseKinematicsTests::testSingleChain() {
|
||||
std::vector<FBXJoint> fbxJoints;
|
||||
makeTestFBXJoints(fbxJoints);
|
||||
FBXGeometry geometry;
|
||||
makeTestFBXJoints(geometry);
|
||||
|
||||
// create a skeleton and doll
|
||||
AnimPose offset;
|
||||
AnimSkeleton* skeleton = new AnimSkeleton(fbxJoints, offset);
|
||||
AnimSkeleton::Pointer skeletonPtr(skeleton);
|
||||
AnimSkeleton::Pointer skeletonPtr = std::make_shared<AnimSkeleton>(geometry);
|
||||
AnimInverseKinematics ikDoll("doll");
|
||||
|
||||
ikDoll.setSkeleton(skeletonPtr);
|
||||
|
||||
{ // easy test IK of joint C
|
||||
|
|
@ -113,11 +113,11 @@ void AnimInverseKinematicsTests::testSingleChain() {
|
|||
pose.rot = identity;
|
||||
pose.trans = origin;
|
||||
|
||||
std::vector<AnimPose> poses;
|
||||
AnimPoseVec poses;
|
||||
poses.push_back(pose);
|
||||
|
||||
pose.trans = xAxis;
|
||||
for (int i = 1; i < (int)fbxJoints.size(); ++i) {
|
||||
for (int i = 1; i < (int)geometry.joints.size(); ++i) {
|
||||
poses.push_back(pose);
|
||||
}
|
||||
ikDoll.loadPoses(poses);
|
||||
|
|
@ -134,7 +134,8 @@ void AnimInverseKinematicsTests::testSingleChain() {
|
|||
AnimVariantMap varMap;
|
||||
varMap.set("positionD", targetPosition);
|
||||
varMap.set("rotationD", targetRotation);
|
||||
ikDoll.setTargetVars("D", "positionD", "rotationD");
|
||||
varMap.set("targetType", (int)IKTarget::Type::RotationAndPosition);
|
||||
ikDoll.setTargetVars(QString("D"), QString("positionD"), QString("rotationD"), QString("targetType"));
|
||||
AnimNode::Triggers triggers;
|
||||
|
||||
// the IK solution should be:
|
||||
|
|
@ -144,38 +145,49 @@ void AnimInverseKinematicsTests::testSingleChain() {
|
|||
// |
|
||||
// A------>B------>C
|
||||
//
|
||||
|
||||
// iterate several times
|
||||
float dt = 1.0f;
|
||||
ikDoll.evaluate(varMap, dt, triggers);
|
||||
ikDoll.overlay(varMap, dt, triggers, poses);
|
||||
ikDoll.overlay(varMap, dt, triggers, poses);
|
||||
ikDoll.overlay(varMap, dt, triggers, poses);
|
||||
|
||||
ikDoll.overlay(varMap, dt, triggers, poses);
|
||||
ikDoll.overlay(varMap, dt, triggers, poses);
|
||||
ikDoll.overlay(varMap, dt, triggers, poses);
|
||||
const AnimPoseVec& relativePoses = ikDoll.overlay(varMap, dt, triggers, poses);
|
||||
|
||||
// verify absolute results
|
||||
// NOTE: since we expect this solution to converge very quickly (one loop)
|
||||
// we can impose very tight error thresholds.
|
||||
std::vector<AnimPose> absolutePoses;
|
||||
AnimPoseVec absolutePoses;
|
||||
for (auto pose : poses) {
|
||||
absolutePoses.push_back(pose);
|
||||
}
|
||||
ikDoll.computeAbsolutePoses(absolutePoses);
|
||||
float acceptableAngle = 0.0001f;
|
||||
QCOMPARE_QUATS(absolutePoses[0].rot, identity, acceptableAngle);
|
||||
QCOMPARE_QUATS(absolutePoses[1].rot, identity, acceptableAngle);
|
||||
QCOMPARE_QUATS(absolutePoses[2].rot, quaterTurnAroundZ, acceptableAngle);
|
||||
QCOMPARE_QUATS(absolutePoses[3].rot, quaterTurnAroundZ, acceptableAngle);
|
||||
const float acceptableAngleError = 0.001f;
|
||||
QCOMPARE_QUATS(absolutePoses[0].rot, identity, acceptableAngleError);
|
||||
QCOMPARE_QUATS(absolutePoses[1].rot, identity, acceptableAngleError);
|
||||
QCOMPARE_QUATS(absolutePoses[2].rot, quaterTurnAroundZ, acceptableAngleError);
|
||||
QCOMPARE_QUATS(absolutePoses[3].rot, quaterTurnAroundZ, acceptableAngleError);
|
||||
|
||||
QCOMPARE_WITH_ABS_ERROR(absolutePoses[0].trans, origin, EPSILON);
|
||||
QCOMPARE_WITH_ABS_ERROR(absolutePoses[1].trans, xAxis, EPSILON);
|
||||
QCOMPARE_WITH_ABS_ERROR(absolutePoses[2].trans, 2.0f * xAxis, EPSILON);
|
||||
QCOMPARE_WITH_ABS_ERROR(absolutePoses[3].trans, targetPosition, EPSILON);
|
||||
const float acceptableTranslationError = 0.025f;
|
||||
QCOMPARE_WITH_ABS_ERROR(absolutePoses[0].trans, origin, acceptableTranslationError);
|
||||
QCOMPARE_WITH_ABS_ERROR(absolutePoses[1].trans, xAxis, acceptableTranslationError);
|
||||
QCOMPARE_WITH_ABS_ERROR(absolutePoses[2].trans, 2.0f * xAxis, acceptableTranslationError);
|
||||
QCOMPARE_WITH_ABS_ERROR(absolutePoses[3].trans, targetPosition, acceptableTranslationError);
|
||||
|
||||
// verify relative results
|
||||
const std::vector<AnimPose>& relativePoses = ikDoll.getRelativePoses();
|
||||
QCOMPARE_QUATS(relativePoses[0].rot, identity, acceptableAngle);
|
||||
QCOMPARE_QUATS(relativePoses[1].rot, identity, acceptableAngle);
|
||||
QCOMPARE_QUATS(relativePoses[2].rot, quaterTurnAroundZ, acceptableAngle);
|
||||
QCOMPARE_QUATS(relativePoses[3].rot, identity, acceptableAngle);
|
||||
QCOMPARE_QUATS(relativePoses[0].rot, identity, acceptableAngleError);
|
||||
QCOMPARE_QUATS(relativePoses[1].rot, identity, acceptableAngleError);
|
||||
QCOMPARE_QUATS(relativePoses[2].rot, quaterTurnAroundZ, acceptableAngleError);
|
||||
QCOMPARE_QUATS(relativePoses[3].rot, identity, acceptableAngleError);
|
||||
|
||||
QCOMPARE_WITH_ABS_ERROR(relativePoses[0].trans, origin, EPSILON);
|
||||
QCOMPARE_WITH_ABS_ERROR(relativePoses[1].trans, xAxis, EPSILON);
|
||||
QCOMPARE_WITH_ABS_ERROR(relativePoses[2].trans, xAxis, EPSILON);
|
||||
QCOMPARE_WITH_ABS_ERROR(relativePoses[3].trans, xAxis, EPSILON);
|
||||
QCOMPARE_WITH_ABS_ERROR(relativePoses[0].trans, origin, acceptableTranslationError);
|
||||
QCOMPARE_WITH_ABS_ERROR(relativePoses[1].trans, xAxis, acceptableTranslationError);
|
||||
QCOMPARE_WITH_ABS_ERROR(relativePoses[2].trans, xAxis, acceptableTranslationError);
|
||||
QCOMPARE_WITH_ABS_ERROR(relativePoses[3].trans, xAxis, acceptableTranslationError);
|
||||
}
|
||||
|
||||
{ // hard test IK of joint C
|
||||
// load intial poses that look like this:
|
||||
//
|
||||
|
|
@ -188,8 +200,8 @@ void AnimInverseKinematicsTests::testSingleChain() {
|
|||
pose.scale = glm::vec3(1.0f);
|
||||
pose.rot = identity;
|
||||
pose.trans = origin;
|
||||
|
||||
std::vector<AnimPose> poses;
|
||||
|
||||
AnimPoseVec poses;
|
||||
poses.push_back(pose);
|
||||
pose.trans = xAxis;
|
||||
|
||||
|
|
@ -211,15 +223,26 @@ void AnimInverseKinematicsTests::testSingleChain() {
|
|||
AnimVariantMap varMap;
|
||||
varMap.set("positionD", targetPosition);
|
||||
varMap.set("rotationD", targetRotation);
|
||||
ikDoll.setTargetVars("D", "positionD", "rotationD");
|
||||
varMap.set("targetType", (int)IKTarget::Type::RotationAndPosition);
|
||||
ikDoll.setTargetVars(QString("D"), QString("positionD"), QString("rotationD"), QString("targetType"));
|
||||
AnimNode::Triggers triggers;
|
||||
|
||||
// the IK solution should be:
|
||||
//
|
||||
// A------>B------>C------>D
|
||||
//
|
||||
|
||||
// iterate several times
|
||||
float dt = 1.0f;
|
||||
ikDoll.evaluate(varMap, dt, triggers);
|
||||
ikDoll.overlay(varMap, dt, triggers, poses);
|
||||
ikDoll.overlay(varMap, dt, triggers, poses);
|
||||
ikDoll.overlay(varMap, dt, triggers, poses);
|
||||
ikDoll.overlay(varMap, dt, triggers, poses);
|
||||
ikDoll.overlay(varMap, dt, triggers, poses);
|
||||
ikDoll.overlay(varMap, dt, triggers, poses);
|
||||
ikDoll.overlay(varMap, dt, triggers, poses);
|
||||
ikDoll.overlay(varMap, dt, triggers, poses);
|
||||
const AnimPoseVec& relativePoses = ikDoll.overlay(varMap, dt, triggers, poses);
|
||||
|
||||
// verify absolute results
|
||||
// NOTE: the IK algorithm doesn't converge very fast for full-reach targets,
|
||||
|
|
@ -228,31 +251,33 @@ void AnimInverseKinematicsTests::testSingleChain() {
|
|||
// NOTE: constraints may help speed up convergence since some joints may get clamped
|
||||
// to maximum extension. TODO: experiment with tightening the error thresholds when
|
||||
// constraints are working.
|
||||
std::vector<AnimPose> absolutePoses;
|
||||
AnimPoseVec absolutePoses;
|
||||
for (auto pose : poses) {
|
||||
absolutePoses.push_back(pose);
|
||||
}
|
||||
ikDoll.computeAbsolutePoses(absolutePoses);
|
||||
float acceptableAngle = 0.1f; // radians
|
||||
float acceptableAngle = 0.01f; // radians
|
||||
QCOMPARE_QUATS(absolutePoses[0].rot, identity, acceptableAngle);
|
||||
QCOMPARE_QUATS(absolutePoses[1].rot, identity, acceptableAngle);
|
||||
QCOMPARE_QUATS(absolutePoses[2].rot, identity, acceptableAngle);
|
||||
QCOMPARE_QUATS(absolutePoses[3].rot, identity, acceptableAngle);
|
||||
|
||||
float acceptableDistance = 0.4f;
|
||||
QCOMPARE_WITH_ABS_ERROR(absolutePoses[0].trans, origin, EPSILON);
|
||||
float acceptableDistance = 0.03f;
|
||||
QCOMPARE_WITH_ABS_ERROR(absolutePoses[0].trans, origin, acceptableDistance);
|
||||
QCOMPARE_WITH_ABS_ERROR(absolutePoses[1].trans, xAxis, acceptableDistance);
|
||||
QCOMPARE_WITH_ABS_ERROR(absolutePoses[2].trans, 2.0f * xAxis, acceptableDistance);
|
||||
QCOMPARE_WITH_ABS_ERROR(absolutePoses[3].trans, 3.0f * xAxis, acceptableDistance);
|
||||
|
||||
// verify relative results
|
||||
const std::vector<AnimPose>& relativePoses = ikDoll.getRelativePoses();
|
||||
QCOMPARE_QUATS(relativePoses[0].rot, identity, acceptableAngle);
|
||||
QCOMPARE_QUATS(relativePoses[1].rot, identity, acceptableAngle);
|
||||
QCOMPARE_QUATS(relativePoses[2].rot, identity, acceptableAngle);
|
||||
QCOMPARE_QUATS(relativePoses[3].rot, identity, acceptableAngle);
|
||||
|
||||
QCOMPARE_WITH_ABS_ERROR(relativePoses[0].trans, origin, EPSILON);
|
||||
QCOMPARE_WITH_ABS_ERROR(relativePoses[1].trans, xAxis, EPSILON);
|
||||
QCOMPARE_WITH_ABS_ERROR(relativePoses[2].trans, xAxis, EPSILON);
|
||||
QCOMPARE_WITH_ABS_ERROR(relativePoses[3].trans, xAxis, EPSILON);
|
||||
QCOMPARE_WITH_ABS_ERROR(relativePoses[0].trans, origin, acceptableDistance);
|
||||
QCOMPARE_WITH_ABS_ERROR(relativePoses[1].trans, xAxis, acceptableDistance);
|
||||
QCOMPARE_WITH_ABS_ERROR(relativePoses[2].trans, xAxis, acceptableDistance);
|
||||
QCOMPARE_WITH_ABS_ERROR(relativePoses[3].trans, xAxis, acceptableDistance);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -38,8 +38,9 @@ void AnimTests::testClipInternalState() {
|
|||
float endFrame = 20.0f;
|
||||
float timeScale = 1.1f;
|
||||
bool loopFlag = true;
|
||||
bool mirrorFlag = false;
|
||||
|
||||
AnimClip clip(id, url, startFrame, endFrame, timeScale, loopFlag);
|
||||
AnimClip clip(id, url, startFrame, endFrame, timeScale, loopFlag, mirrorFlag);
|
||||
|
||||
QVERIFY(clip.getID() == id);
|
||||
QVERIFY(clip.getType() == AnimNode::Type::Clip);
|
||||
|
|
@ -49,6 +50,7 @@ void AnimTests::testClipInternalState() {
|
|||
QVERIFY(clip._endFrame == endFrame);
|
||||
QVERIFY(clip._timeScale == timeScale);
|
||||
QVERIFY(clip._loopFlag == loopFlag);
|
||||
QVERIFY(clip._mirrorFlag == mirrorFlag);
|
||||
}
|
||||
|
||||
static float framesToSec(float secs) {
|
||||
|
|
@ -62,12 +64,13 @@ void AnimTests::testClipEvaulate() {
|
|||
float startFrame = 2.0f;
|
||||
float endFrame = 22.0f;
|
||||
float timeScale = 1.0f;
|
||||
float loopFlag = true;
|
||||
bool loopFlag = true;
|
||||
bool mirrorFlag = false;
|
||||
|
||||
auto vars = AnimVariantMap();
|
||||
vars.set("FalseVar", false);
|
||||
|
||||
AnimClip clip(id, url, startFrame, endFrame, timeScale, loopFlag);
|
||||
AnimClip clip(id, url, startFrame, endFrame, timeScale, loopFlag, mirrorFlag);
|
||||
|
||||
AnimNode::Triggers triggers;
|
||||
clip.evaluate(vars, framesToSec(10.0f), triggers);
|
||||
|
|
@ -97,7 +100,8 @@ void AnimTests::testClipEvaulateWithVars() {
|
|||
float startFrame = 2.0f;
|
||||
float endFrame = 22.0f;
|
||||
float timeScale = 1.0f;
|
||||
float loopFlag = true;
|
||||
bool loopFlag = true;
|
||||
bool mirrorFlag = false;
|
||||
|
||||
float startFrame2 = 22.0f;
|
||||
float endFrame2 = 100.0f;
|
||||
|
|
@ -110,7 +114,7 @@ void AnimTests::testClipEvaulateWithVars() {
|
|||
vars.set("timeScale2", timeScale2);
|
||||
vars.set("loopFlag2", loopFlag2);
|
||||
|
||||
AnimClip clip(id, url, startFrame, endFrame, timeScale, loopFlag);
|
||||
AnimClip clip(id, url, startFrame, endFrame, timeScale, loopFlag, mirrorFlag);
|
||||
clip.setStartFrameVar("startFrame2");
|
||||
clip.setEndFrameVar("endFrame2");
|
||||
clip.setTimeScaleVar("timeScale2");
|
||||
|
|
@ -583,23 +587,23 @@ void AnimTests::testExpressionEvaluator() {
|
|||
TEST_BOOL_EXPR(false && false);
|
||||
TEST_BOOL_EXPR(false && true);
|
||||
|
||||
TEST_BOOL_EXPR(true || false && true);
|
||||
TEST_BOOL_EXPR(true || false && false);
|
||||
TEST_BOOL_EXPR(true || true && true);
|
||||
TEST_BOOL_EXPR(true || true && false);
|
||||
TEST_BOOL_EXPR(false || false && true);
|
||||
TEST_BOOL_EXPR(false || false && false);
|
||||
TEST_BOOL_EXPR(false || true && true);
|
||||
TEST_BOOL_EXPR(false || true && false);
|
||||
TEST_BOOL_EXPR(true || (false && true));
|
||||
TEST_BOOL_EXPR(true || (false && false));
|
||||
TEST_BOOL_EXPR(true || (true && true));
|
||||
TEST_BOOL_EXPR(true || (true && false));
|
||||
TEST_BOOL_EXPR(false || (false && true));
|
||||
TEST_BOOL_EXPR(false || (false && false));
|
||||
TEST_BOOL_EXPR(false || (true && true));
|
||||
TEST_BOOL_EXPR(false || (true && false));
|
||||
|
||||
TEST_BOOL_EXPR(true && false || true);
|
||||
TEST_BOOL_EXPR(true && false || false);
|
||||
TEST_BOOL_EXPR(true && true || true);
|
||||
TEST_BOOL_EXPR(true && true || false);
|
||||
TEST_BOOL_EXPR(false && false || true);
|
||||
TEST_BOOL_EXPR(false && false || false);
|
||||
TEST_BOOL_EXPR(false && true || true);
|
||||
TEST_BOOL_EXPR(false && true || false);
|
||||
TEST_BOOL_EXPR((true && false) || true);
|
||||
TEST_BOOL_EXPR((true && false) || false);
|
||||
TEST_BOOL_EXPR((true && true) || true);
|
||||
TEST_BOOL_EXPR((true && true) || false);
|
||||
TEST_BOOL_EXPR((false && false) || true);
|
||||
TEST_BOOL_EXPR((false && false) || false);
|
||||
TEST_BOOL_EXPR((false && true) || true);
|
||||
TEST_BOOL_EXPR((false && true) || false);
|
||||
|
||||
TEST_BOOL_EXPR(t || false);
|
||||
TEST_BOOL_EXPR(t || true);
|
||||
|
|
@ -610,14 +614,14 @@ void AnimTests::testExpressionEvaluator() {
|
|||
TEST_BOOL_EXPR(!false);
|
||||
TEST_BOOL_EXPR(!true || true);
|
||||
|
||||
TEST_BOOL_EXPR(!true && !false || !true);
|
||||
TEST_BOOL_EXPR(!true && !false || true);
|
||||
TEST_BOOL_EXPR(!true && false || !true);
|
||||
TEST_BOOL_EXPR(!true && false || true);
|
||||
TEST_BOOL_EXPR(true && !false || !true);
|
||||
TEST_BOOL_EXPR(true && !false || true);
|
||||
TEST_BOOL_EXPR(true && false || !true);
|
||||
TEST_BOOL_EXPR(true && false || true);
|
||||
TEST_BOOL_EXPR((!true && !false) || !true);
|
||||
TEST_BOOL_EXPR((!true && !false) || true);
|
||||
TEST_BOOL_EXPR((!true && false) || !true);
|
||||
TEST_BOOL_EXPR((!true && false) || true);
|
||||
TEST_BOOL_EXPR((true && !false) || !true);
|
||||
TEST_BOOL_EXPR((true && !false) || true);
|
||||
TEST_BOOL_EXPR((true && false) || !true);
|
||||
TEST_BOOL_EXPR((true && false) || true);
|
||||
|
||||
TEST_BOOL_EXPR(!(true && f) || !t);
|
||||
TEST_BOOL_EXPR(!!!(t) && (!!f || true));
|
||||
|
|
|
|||
|
|
@ -13,6 +13,7 @@
|
|||
#include <glm/glm.hpp>
|
||||
|
||||
#include <ElbowConstraint.h>
|
||||
#include <GLMHelpers.h>
|
||||
#include <NumericalConstants.h>
|
||||
#include <SwingTwistConstraint.h>
|
||||
|
||||
|
|
@ -56,7 +57,7 @@ void RotationConstraintTests::testElbowConstraint() {
|
|||
float startAngle = minAngle + smallAngle;
|
||||
float endAngle = maxAngle - smallAngle;
|
||||
float deltaAngle = (endAngle - startAngle) / (float)(numChecks - 1);
|
||||
|
||||
|
||||
for (float angle = startAngle; angle < endAngle + 0.5f * deltaAngle; angle += deltaAngle) {
|
||||
glm::quat inputRotation = glm::angleAxis(angle, hingeAxis) * referenceRotation;
|
||||
glm::quat outputRotation = inputRotation;
|
||||
|
|
@ -115,9 +116,9 @@ void RotationConstraintTests::testSwingTwistConstraint() {
|
|||
shoulder.setTwistLimits(minTwistAngle, maxTwistAngle);
|
||||
float lowDot = 0.25f;
|
||||
float highDot = 0.75f;
|
||||
// The swing constriants are more interesting: a vector of minimum dot products
|
||||
// The swing constriants are more interesting: a vector of minimum dot products
|
||||
// as a function of theta around the twist axis. Our test function will be shaped
|
||||
// like the square wave with amplitudes 0.25 and 0.75:
|
||||
// like a square wave with amplitudes 0.25 and 0.75:
|
||||
//
|
||||
// |
|
||||
// 0.75 - o---o---o---o
|
||||
|
|
@ -308,3 +309,344 @@ void RotationConstraintTests::testSwingTwistConstraint() {
|
|||
}
|
||||
}
|
||||
|
||||
void RotationConstraintTests::testDynamicSwingLimitFunction() {
|
||||
SwingTwistConstraint::SwingLimitFunction limitFunction;
|
||||
const float ACCEPTABLE_ERROR = 1.0e-6f;
|
||||
|
||||
const float adjustmentDot = -0.5f;
|
||||
|
||||
const float MIN_DOT = 0.5f;
|
||||
{ // initialize limitFunction
|
||||
std::vector<float> minDots;
|
||||
minDots.push_back(MIN_DOT);
|
||||
limitFunction.setMinDots(minDots);
|
||||
}
|
||||
|
||||
std::vector<float> referenceDots;
|
||||
{ // verify limits and initialize referenceDots
|
||||
const int MIN_NUM_DOTS = 8;
|
||||
const std::vector<float>& minDots = limitFunction.getMinDots();
|
||||
QVERIFY(minDots.size() >= MIN_NUM_DOTS);
|
||||
|
||||
int numDots = (int)minDots.size();
|
||||
for (int i = 0; i < numDots; ++i) {
|
||||
QCOMPARE_WITH_RELATIVE_ERROR(minDots[i], MIN_DOT, ACCEPTABLE_ERROR);
|
||||
referenceDots.push_back(minDots[i]);
|
||||
}
|
||||
}
|
||||
{ // dynamically adjust limits
|
||||
const std::vector<float>& minDots = limitFunction.getMinDots();
|
||||
int numDots = (int)minDots.size();
|
||||
|
||||
float deltaTheta = TWO_PI / (float)(numDots - 1);
|
||||
int indexA = 2;
|
||||
int indexB = (indexA + 1) % numDots;
|
||||
|
||||
{ // dynamically adjust a data point
|
||||
float theta = deltaTheta * (float)indexA;
|
||||
float interpolatedDot = limitFunction.getMinDot(theta);
|
||||
|
||||
// change indexA
|
||||
limitFunction.dynamicallyAdjustMinDots(theta, adjustmentDot);
|
||||
QCOMPARE_WITH_ABS_ERROR(limitFunction.getMinDot(theta), adjustmentDot, ACCEPTABLE_ERROR); // adjustmentDot at theta
|
||||
QCOMPARE_WITH_ABS_ERROR(minDots[indexA], adjustmentDot, ACCEPTABLE_ERROR); // indexA has changed
|
||||
QCOMPARE_WITH_ABS_ERROR(minDots[indexB], referenceDots[indexB], ACCEPTABLE_ERROR); // indexB has not changed
|
||||
|
||||
// change indexB
|
||||
theta = deltaTheta * (float)indexB;
|
||||
limitFunction.dynamicallyAdjustMinDots(theta, adjustmentDot);
|
||||
QCOMPARE_WITH_ABS_ERROR(limitFunction.getMinDot(theta), adjustmentDot, ACCEPTABLE_ERROR); // adjustmentDot at theta
|
||||
QCOMPARE_WITH_ABS_ERROR(minDots[indexA], referenceDots[indexA], ACCEPTABLE_ERROR); // indexA has been restored
|
||||
QCOMPARE_WITH_ABS_ERROR(minDots[indexB], adjustmentDot, ACCEPTABLE_ERROR); // indexB has changed
|
||||
|
||||
// restore
|
||||
limitFunction.dynamicallyAdjustMinDots(theta, referenceDots[indexB] + 0.01f); // restore with a larger dot
|
||||
QCOMPARE_WITH_ABS_ERROR(limitFunction.getMinDot(theta), interpolatedDot, ACCEPTABLE_ERROR); // restored
|
||||
QCOMPARE_WITH_ABS_ERROR(minDots[indexA], referenceDots[indexA], ACCEPTABLE_ERROR); // indexA is restored
|
||||
QCOMPARE_WITH_ABS_ERROR(minDots[indexB], referenceDots[indexB], ACCEPTABLE_ERROR); // indexB is restored
|
||||
}
|
||||
{ // dynamically adjust halfway between data points
|
||||
float theta = deltaTheta * 0.5f * (float)(indexA + indexB); // halfway between two points
|
||||
float interpolatedDot = limitFunction.getMinDot(theta);
|
||||
float deltaDot = adjustmentDot - interpolatedDot;
|
||||
limitFunction.dynamicallyAdjustMinDots(theta, adjustmentDot);
|
||||
QCOMPARE_WITH_ABS_ERROR(limitFunction.getMinDot(theta), adjustmentDot, ACCEPTABLE_ERROR); // adjustmentDot at theta
|
||||
QCOMPARE_WITH_ABS_ERROR(minDots[indexA], referenceDots[indexA] + deltaDot, ACCEPTABLE_ERROR); // indexA has changed
|
||||
QCOMPARE_WITH_ABS_ERROR(minDots[indexB], referenceDots[indexB] + deltaDot, ACCEPTABLE_ERROR); // indexB has changed
|
||||
|
||||
limitFunction.dynamicallyAdjustMinDots(theta, interpolatedDot + 0.01f); // reset with something larger
|
||||
QCOMPARE_WITH_ABS_ERROR(limitFunction.getMinDot(theta), interpolatedDot, ACCEPTABLE_ERROR); // restored
|
||||
QCOMPARE_WITH_ABS_ERROR(minDots[indexA], referenceDots[indexA], ACCEPTABLE_ERROR); // indexA is restored
|
||||
QCOMPARE_WITH_ABS_ERROR(minDots[indexB], referenceDots[indexB], ACCEPTABLE_ERROR); // indexB is restored
|
||||
}
|
||||
{ // dynamically adjust one-quarter between data points
|
||||
float theta = deltaTheta * ((float)indexA + 0.25f); // one quarter past A towards B
|
||||
float interpolatedDot = limitFunction.getMinDot(theta);
|
||||
limitFunction.dynamicallyAdjustMinDots(theta, adjustmentDot);
|
||||
QCOMPARE_WITH_ABS_ERROR(limitFunction.getMinDot(theta), adjustmentDot, ACCEPTABLE_ERROR); // adjustmentDot at theta
|
||||
QVERIFY(minDots[indexA] < adjustmentDot); // indexA should be less than minDot
|
||||
QVERIFY(minDots[indexB] > adjustmentDot); // indexB should be larger than minDot
|
||||
QVERIFY(minDots[indexB] < referenceDots[indexB]); // indexB should be less than what it was
|
||||
|
||||
limitFunction.dynamicallyAdjustMinDots(theta, interpolatedDot + 0.01f); // reset with something larger
|
||||
QCOMPARE_WITH_ABS_ERROR(limitFunction.getMinDot(theta), interpolatedDot, ACCEPTABLE_ERROR); // restored
|
||||
QCOMPARE_WITH_ABS_ERROR(minDots[indexA], referenceDots[indexA], ACCEPTABLE_ERROR); // indexA is restored
|
||||
QCOMPARE_WITH_ABS_ERROR(minDots[indexB], referenceDots[indexB], ACCEPTABLE_ERROR); // indexB is restored
|
||||
}
|
||||
{ // halfway between first two data points (boundary condition)
|
||||
indexA = 0;
|
||||
indexB = 1;
|
||||
int indexZ = minDots.size() - 1; // far boundary condition
|
||||
float theta = deltaTheta * 0.5f * (float)(indexA + indexB); // halfway between two points
|
||||
float interpolatedDot = limitFunction.getMinDot(theta);
|
||||
float deltaDot = adjustmentDot - interpolatedDot;
|
||||
limitFunction.dynamicallyAdjustMinDots(theta, adjustmentDot);
|
||||
QCOMPARE_WITH_ABS_ERROR(limitFunction.getMinDot(theta), adjustmentDot, ACCEPTABLE_ERROR); // adjustmentDot at theta
|
||||
QCOMPARE_WITH_ABS_ERROR(minDots[indexA], referenceDots[indexA] + deltaDot, ACCEPTABLE_ERROR); // indexA has changed
|
||||
QCOMPARE_WITH_ABS_ERROR(minDots[indexB], referenceDots[indexB] + deltaDot, ACCEPTABLE_ERROR); // indexB has changed
|
||||
QCOMPARE_WITH_ABS_ERROR(minDots[indexZ], referenceDots[indexZ] + deltaDot, ACCEPTABLE_ERROR); // indexZ has changed
|
||||
|
||||
limitFunction.dynamicallyAdjustMinDots(theta, interpolatedDot + 0.01f); // reset with something larger
|
||||
QCOMPARE_WITH_ABS_ERROR(limitFunction.getMinDot(theta), interpolatedDot, ACCEPTABLE_ERROR); // restored
|
||||
QCOMPARE_WITH_ABS_ERROR(minDots[indexA], referenceDots[indexA], ACCEPTABLE_ERROR); // indexA is restored
|
||||
QCOMPARE_WITH_ABS_ERROR(minDots[indexB], referenceDots[indexB], ACCEPTABLE_ERROR); // indexB is restored
|
||||
QCOMPARE_WITH_ABS_ERROR(minDots[indexZ], referenceDots[indexZ], ACCEPTABLE_ERROR); // indexZ is restored
|
||||
}
|
||||
{ // halfway between first two data points (boundary condition)
|
||||
indexB = minDots.size() - 1;
|
||||
indexA = indexB - 1;
|
||||
int indexZ = 0; // far boundary condition
|
||||
float theta = deltaTheta * 0.5f * (float)(indexA + indexB); // halfway between two points
|
||||
float interpolatedDot = limitFunction.getMinDot(theta);
|
||||
float deltaDot = adjustmentDot - interpolatedDot;
|
||||
limitFunction.dynamicallyAdjustMinDots(theta, adjustmentDot);
|
||||
QCOMPARE_WITH_ABS_ERROR(limitFunction.getMinDot(theta), adjustmentDot, ACCEPTABLE_ERROR); // adjustmentDot at theta
|
||||
QCOMPARE_WITH_ABS_ERROR(minDots[indexA], referenceDots[indexA] + deltaDot, ACCEPTABLE_ERROR); // indexA has changed
|
||||
QCOMPARE_WITH_ABS_ERROR(minDots[indexB], referenceDots[indexB] + deltaDot, ACCEPTABLE_ERROR); // indexB has changed
|
||||
QCOMPARE_WITH_ABS_ERROR(minDots[indexZ], referenceDots[indexZ] + deltaDot, ACCEPTABLE_ERROR); // indexZ has changed
|
||||
|
||||
limitFunction.dynamicallyAdjustMinDots(theta, interpolatedDot + 0.01f); // reset with something larger
|
||||
QCOMPARE_WITH_ABS_ERROR(limitFunction.getMinDot(theta), interpolatedDot, ACCEPTABLE_ERROR); // restored
|
||||
QCOMPARE_WITH_ABS_ERROR(minDots[indexA], referenceDots[indexA], ACCEPTABLE_ERROR); // indexA is restored
|
||||
QCOMPARE_WITH_ABS_ERROR(minDots[indexB], referenceDots[indexB], ACCEPTABLE_ERROR); // indexB is restored
|
||||
QCOMPARE_WITH_ABS_ERROR(minDots[indexZ], referenceDots[indexZ], ACCEPTABLE_ERROR); // indexZ is restored
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void RotationConstraintTests::testDynamicSwing() {
|
||||
const float ACCEPTABLE_ERROR = 1.0e-6f;
|
||||
|
||||
// referenceRotation is the default rotation
|
||||
float referenceAngle = 1.23f;
|
||||
glm::vec3 referenceAxis = glm::normalize(glm::vec3(1.0f, 2.0f, -3.0f));
|
||||
glm::quat referenceRotation = glm::angleAxis(referenceAngle, referenceAxis);
|
||||
|
||||
// the angle limits of the constriant about the hinge axis
|
||||
float minTwistAngle = -PI / 2.0f;
|
||||
float maxTwistAngle = PI / 2.0f;
|
||||
|
||||
// build the constraint
|
||||
SwingTwistConstraint shoulder;
|
||||
shoulder.setReferenceRotation(referenceRotation);
|
||||
shoulder.setTwistLimits(minTwistAngle, maxTwistAngle);
|
||||
std::vector<float> minDots;
|
||||
const float MIN_DOT = 0.5f;
|
||||
minDots.push_back(MIN_DOT);
|
||||
shoulder.setSwingLimits(minDots);
|
||||
|
||||
// verify resolution of the swing limits
|
||||
const std::vector<float>& shoulderMinDots = shoulder.getMinDots();
|
||||
const int MIN_NUM_DOTS = 8;
|
||||
int numDots = shoulderMinDots.size();
|
||||
QVERIFY(numDots >= MIN_NUM_DOTS);
|
||||
|
||||
// verify values of the swing limits
|
||||
QCOMPARE_WITH_ABS_ERROR(shoulderMinDots[0], shoulderMinDots[numDots - 1], ACCEPTABLE_ERROR); // endpoints should be the same
|
||||
for (int i = 0; i < numDots; ++i) {
|
||||
QCOMPARE_WITH_ABS_ERROR(shoulderMinDots[i], MIN_DOT, ACCEPTABLE_ERROR); // all values should be the same
|
||||
}
|
||||
|
||||
float deltaTheta = TWO_PI / (float)(numDots - 1);
|
||||
float theta = 1.5f * deltaTheta;
|
||||
glm::vec3 swingAxis(cosf(theta), 0.0f, sinf(theta));
|
||||
float deltaSwing = 0.1f;
|
||||
|
||||
{ // compute rotation that should NOT be constrained
|
||||
float swingAngle = acosf(MIN_DOT) - deltaSwing;
|
||||
glm::quat swingRotation = glm::angleAxis(swingAngle, swingAxis);
|
||||
glm::quat totalRotation = swingRotation * referenceRotation;
|
||||
|
||||
// verify rotation is NOT constrained
|
||||
glm::quat constrainedRotation = totalRotation;
|
||||
QVERIFY(!shoulder.apply(constrainedRotation));
|
||||
}
|
||||
|
||||
{ // compute a rotation that should be barely constrained
|
||||
float swingAngle = acosf(MIN_DOT) + deltaSwing;
|
||||
glm::quat swingRotation = glm::angleAxis(swingAngle, swingAxis);
|
||||
glm::quat totalRotation = swingRotation * referenceRotation;
|
||||
|
||||
// verify rotation is constrained
|
||||
glm::quat constrainedRotation = totalRotation;
|
||||
QVERIFY(shoulder.apply(constrainedRotation));
|
||||
}
|
||||
|
||||
{ // make a dynamic adjustment to the swing limits
|
||||
const float SMALLER_MIN_DOT = -0.5f;
|
||||
float swingAngle = acosf(SMALLER_MIN_DOT);
|
||||
glm::quat swingRotation = glm::angleAxis(swingAngle, swingAxis);
|
||||
glm::quat badRotation = swingRotation * referenceRotation;
|
||||
|
||||
{ // verify rotation is constrained
|
||||
glm::quat constrainedRotation = badRotation;
|
||||
QVERIFY(shoulder.apply(constrainedRotation));
|
||||
|
||||
// now poke the SMALLER_MIN_DOT into the swing limits
|
||||
shoulder.dynamicallyAdjustLimits(badRotation);
|
||||
|
||||
// verify that if rotation is constrained then it is only by a little bit
|
||||
constrainedRotation = badRotation;
|
||||
bool constrained = shoulder.apply(constrainedRotation);
|
||||
if (constrained) {
|
||||
// Note: Q1 = dQ * Q0 --> dQ = Q1 * Q0^
|
||||
glm::quat dQ = constrainedRotation * glm::inverse(badRotation);
|
||||
const float acceptableClampAngle = 0.01f;
|
||||
float deltaAngle = glm::angle(dQ);
|
||||
QVERIFY(deltaAngle < acceptableClampAngle);
|
||||
}
|
||||
}
|
||||
|
||||
{ // verify that other swing axes still use the old non-adjusted limits
|
||||
float deltaTheta = TWO_PI / (float)(numDots - 1);
|
||||
float otherTheta = 3.5f * deltaTheta;
|
||||
glm::vec3 otherSwingAxis(cosf(otherTheta), 0.0f, sinf(otherTheta));
|
||||
|
||||
{ // inside rotations should be unconstrained
|
||||
float goodAngle = acosf(MIN_DOT) - deltaSwing;
|
||||
glm::quat goodRotation = glm::angleAxis(goodAngle, otherSwingAxis) * referenceRotation;
|
||||
QVERIFY(!shoulder.apply(goodRotation));
|
||||
}
|
||||
{ // outside rotations should be constrained
|
||||
float badAngle = acosf(MIN_DOT) + deltaSwing;
|
||||
glm::quat otherBadRotation = glm::angleAxis(badAngle, otherSwingAxis) * referenceRotation;
|
||||
QVERIFY(shoulder.apply(otherBadRotation));
|
||||
|
||||
float constrainedAngle = glm::angle(otherBadRotation);
|
||||
QCOMPARE_WITH_ABS_ERROR(constrainedAngle, acosf(MIN_DOT), 0.1f * deltaSwing);
|
||||
}
|
||||
}
|
||||
|
||||
{ // clear dynamic adjustment
|
||||
float goodAngle = acosf(MIN_DOT) - deltaSwing;
|
||||
glm::quat goodRotation = glm::angleAxis(goodAngle, swingAxis) * referenceRotation;
|
||||
|
||||
// when we update with a goodRotation the dynamic adjustment is cleared
|
||||
shoulder.dynamicallyAdjustLimits(goodRotation);
|
||||
|
||||
// verify that the old badRotation, which was not constrained dynamically, is now constrained
|
||||
glm::quat constrainedRotation = badRotation;
|
||||
QVERIFY(shoulder.apply(constrainedRotation));
|
||||
|
||||
// and the good rotation should not be constrained
|
||||
constrainedRotation = goodRotation;
|
||||
QVERIFY(!shoulder.apply(constrainedRotation));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void RotationConstraintTests::testDynamicTwist() {
|
||||
// referenceRotation is the default rotation
|
||||
float referenceAngle = 1.23f;
|
||||
glm::vec3 referenceAxis = glm::normalize(glm::vec3(1.0f, 2.0f, -3.0f));
|
||||
glm::quat referenceRotation = glm::angleAxis(referenceAngle, referenceAxis);
|
||||
|
||||
// the angle limits of the constriant about the hinge axis
|
||||
const float minTwistAngle = -PI / 2.0f;
|
||||
const float maxTwistAngle = PI / 2.0f;
|
||||
|
||||
// build the constraint
|
||||
SwingTwistConstraint shoulder;
|
||||
shoulder.setReferenceRotation(referenceRotation);
|
||||
shoulder.setTwistLimits(minTwistAngle, maxTwistAngle);
|
||||
|
||||
glm::vec3 twistAxis = Vectors::UNIT_Y;
|
||||
float deltaTwist = 0.1f;
|
||||
|
||||
{ // compute min rotation that should NOT be constrained
|
||||
float twistAngle = minTwistAngle + deltaTwist;
|
||||
glm::quat twistRotation = glm::angleAxis(twistAngle, twistAxis);
|
||||
glm::quat totalRotation = twistRotation * referenceRotation;
|
||||
|
||||
// verify rotation is NOT constrained
|
||||
glm::quat constrainedRotation = totalRotation;
|
||||
QVERIFY(!shoulder.apply(constrainedRotation));
|
||||
}
|
||||
{ // compute max rotation that should NOT be constrained
|
||||
float twistAngle = maxTwistAngle - deltaTwist;
|
||||
glm::quat twistRotation = glm::angleAxis(twistAngle, twistAxis);
|
||||
glm::quat totalRotation = twistRotation * referenceRotation;
|
||||
|
||||
// verify rotation is NOT constrained
|
||||
glm::quat constrainedRotation = totalRotation;
|
||||
QVERIFY(!shoulder.apply(constrainedRotation));
|
||||
}
|
||||
{ // compute a min rotation that should be barely constrained
|
||||
float twistAngle = minTwistAngle - deltaTwist;
|
||||
glm::quat twistRotation = glm::angleAxis(twistAngle, twistAxis);
|
||||
glm::quat totalRotation = twistRotation * referenceRotation;
|
||||
|
||||
// verify rotation is constrained
|
||||
glm::quat constrainedRotation = totalRotation;
|
||||
QVERIFY(shoulder.apply(constrainedRotation));
|
||||
|
||||
// adjust the constraint and verify rotation is NOT constrained
|
||||
shoulder.dynamicallyAdjustLimits(totalRotation);
|
||||
constrainedRotation = totalRotation;
|
||||
bool constrained = shoulder.apply(constrainedRotation);
|
||||
if (constrained) {
|
||||
// or, if it is constrained then the adjustment is very small
|
||||
// Note: Q1 = dQ * Q0 --> dQ = Q1 * Q0^
|
||||
glm::quat dQ = constrainedRotation * glm::inverse(totalRotation);
|
||||
const float acceptableClampAngle = 0.01f;
|
||||
float deltaAngle = glm::angle(dQ);
|
||||
QVERIFY(deltaAngle < acceptableClampAngle);
|
||||
}
|
||||
|
||||
// clear the adjustment using a null rotation
|
||||
shoulder.dynamicallyAdjustLimits(glm::quat());
|
||||
|
||||
// verify that rotation is constrained again
|
||||
constrainedRotation = totalRotation;
|
||||
QVERIFY(shoulder.apply(constrainedRotation));
|
||||
}
|
||||
{ // compute a min rotation that should be barely constrained
|
||||
float twistAngle = maxTwistAngle + deltaTwist;
|
||||
glm::quat twistRotation = glm::angleAxis(twistAngle, twistAxis);
|
||||
glm::quat totalRotation = twistRotation * referenceRotation;
|
||||
|
||||
// verify rotation is constrained
|
||||
glm::quat constrainedRotation = totalRotation;
|
||||
QVERIFY(shoulder.apply(constrainedRotation));
|
||||
|
||||
// adjust the constraint and verify rotation is NOT constrained
|
||||
shoulder.dynamicallyAdjustLimits(totalRotation);
|
||||
constrainedRotation = totalRotation;
|
||||
bool constrained = shoulder.apply(constrainedRotation);
|
||||
if (constrained) {
|
||||
// or, if it is constrained then the adjustment is very small
|
||||
// Note: Q1 = dQ * Q0 --> dQ = Q1 * Q0^
|
||||
glm::quat dQ = constrainedRotation * glm::inverse(totalRotation);
|
||||
const float acceptableClampAngle = 0.01f;
|
||||
float deltaAngle = glm::angle(dQ);
|
||||
QVERIFY(deltaAngle < acceptableClampAngle);
|
||||
}
|
||||
|
||||
// clear the adjustment using a null rotation
|
||||
shoulder.dynamicallyAdjustLimits(glm::quat());
|
||||
|
||||
// verify that rotation is constrained again
|
||||
constrainedRotation = totalRotation;
|
||||
QVERIFY(shoulder.apply(constrainedRotation));
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -15,10 +15,13 @@
|
|||
|
||||
class RotationConstraintTests : public QObject {
|
||||
Q_OBJECT
|
||||
|
||||
|
||||
private slots:
|
||||
void testElbowConstraint();
|
||||
void testSwingTwistConstraint();
|
||||
void testDynamicSwingLimitFunction();
|
||||
void testDynamicSwing();
|
||||
void testDynamicTwist();
|
||||
};
|
||||
|
||||
#endif // hifi_RotationConstraintTests_h
|
||||
|
|
|
|||
Loading…
Reference in a new issue