From 356a29c2fbb82f2a3080e4b71f7e66f177ff9536 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Thu, 3 Jul 2014 08:40:34 -0700 Subject: [PATCH] add AngularConstraint::softClamp() for hands --- interface/src/renderer/JointState.cpp | 13 +++- interface/src/renderer/JointState.h | 2 +- interface/src/renderer/Model.cpp | 2 +- libraries/shared/src/AngularConstraint.cpp | 34 +++++++-- libraries/shared/src/AngularConstraint.h | 8 +- tests/shared/src/AngularConstraintTests.cpp | 84 ++++++++++----------- 6 files changed, 88 insertions(+), 55 deletions(-) diff --git a/interface/src/renderer/JointState.cpp b/interface/src/renderer/JointState.cpp index 6cb58f0664..429084480d 100644 --- a/interface/src/renderer/JointState.cpp +++ b/interface/src/renderer/JointState.cpp @@ -24,9 +24,9 @@ JointState::JointState() : } JointState::JointState(const JointState& other) : _constraint(NULL) { - _rotationInParentFrame = other._rotationInParentFrame; _transform = other._transform; _rotation = other._rotation; + _rotationInParentFrame = other._rotationInParentFrame; _animationPriority = other._animationPriority; _fbxJoint = other._fbxJoint; // DO NOT copy _constraint @@ -102,11 +102,15 @@ void JointState::restoreRotation(float fraction, float priority) { } } -void JointState::setRotationFromBindFrame(const glm::quat& rotation, float priority) { +void JointState::setRotationFromBindFrame(const glm::quat& rotation, float priority, bool constrain) { // rotation is from bind- to model-frame assert(_fbxJoint != NULL); if (priority >= _animationPriority) { - setRotationInParentFrame(_rotationInParentFrame * glm::inverse(_rotation) * rotation * glm::inverse(_fbxJoint->inverseBindRotation)); + glm::quat targetRotation = _rotationInParentFrame * glm::inverse(_rotation) * rotation * glm::inverse(_fbxJoint->inverseBindRotation); + if (constrain && _constraint) { + _constraint->softClamp(targetRotation, _rotationInParentFrame, 0.5f); + } + setRotationInParentFrame(targetRotation); _animationPriority = priority; } } @@ -154,6 +158,9 @@ void JointState::mixRotationDelta(const glm::quat& delta, float mixFactor, float if (mixFactor > 0.0f && mixFactor <= 1.0f) { targetRotation = safeMix(targetRotation, _fbxJoint->rotation, mixFactor); } + if (_constraint) { + _constraint->softClamp(targetRotation, _rotationInParentFrame, 0.5f); + } setRotationInParentFrame(targetRotation); } diff --git a/interface/src/renderer/JointState.h b/interface/src/renderer/JointState.h index 5e8c8704fc..3bd752cdff 100644 --- a/interface/src/renderer/JointState.h +++ b/interface/src/renderer/JointState.h @@ -68,7 +68,7 @@ public: /// \param rotation is from bind- to model-frame /// computes and sets new _rotationInParentFrame /// NOTE: the JointState's model-frame transform/rotation are NOT updated! - void setRotationFromBindFrame(const glm::quat& rotation, float priority); + void setRotationFromBindFrame(const glm::quat& rotation, float priority, bool constrain = false); void setRotationInParentFrame(const glm::quat& targetRotation); const glm::quat& getRotationInParentFrame() const { return _rotationInParentFrame; } diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp index c47fd0893f..1d822c7ed1 100644 --- a/interface/src/renderer/Model.cpp +++ b/interface/src/renderer/Model.cpp @@ -1216,7 +1216,7 @@ void Model::inverseKinematics(int endIndex, glm::vec3 targetPosition, const glm: } while (numIterations < MAX_ITERATION_COUNT && distanceToGo < ACCEPTABLE_IK_ERROR); // set final rotation of the end joint - endState.setRotationFromBindFrame(targetRotation, priority); + endState.setRotationFromBindFrame(targetRotation, priority, true); _shapesAreDirty = !_shapes.isEmpty(); } diff --git a/libraries/shared/src/AngularConstraint.cpp b/libraries/shared/src/AngularConstraint.cpp index 48649450e1..4689568ac8 100644 --- a/libraries/shared/src/AngularConstraint.cpp +++ b/libraries/shared/src/AngularConstraint.cpp @@ -74,6 +74,26 @@ AngularConstraint* AngularConstraint::newAngularConstraint(const glm::vec3& minA return NULL; } +bool AngularConstraint::softClamp(glm::quat& targetRotation, const glm::quat& oldRotation, float mixFraction) { + glm::quat clampedTarget = targetRotation; + bool clamped = clamp(clampedTarget); + if (clamped) { + // check if oldRotation is also clamped + glm::quat clampedOld = oldRotation; + bool clamped2 = clamp(clampedOld); + if (clamped2) { + // oldRotation is already beyond the constraint + // we clamp again midway between targetRotation and clamped oldPosition + clampedTarget = glm::shortMix(clampedOld, targetRotation, mixFraction); + // and then clamp that + clamp(clampedTarget); + } + // finally we mix targetRotation with the clampedTarget + targetRotation = glm::shortMix(clampedTarget, targetRotation, mixFraction); + } + return clamped; +} + HingeConstraint::HingeConstraint(const glm::vec3& forwardAxis, const glm::vec3& rotationAxis, float minAngle, float maxAngle) : _minAngle(minAngle), _maxAngle(maxAngle) { assert(_minAngle < _maxAngle); @@ -87,7 +107,7 @@ HingeConstraint::HingeConstraint(const glm::vec3& forwardAxis, const glm::vec3& } // virtual -bool HingeConstraint::applyTo(glm::quat& rotation) const { +bool HingeConstraint::clamp(glm::quat& rotation) const { glm::vec3 forward = rotation * _forwardAxis; forward -= glm::dot(forward, _rotationAxis) * _rotationAxis; float length = glm::length(forward); @@ -108,6 +128,11 @@ bool HingeConstraint::applyTo(glm::quat& rotation) const { return false; } +bool HingeConstraint::softClamp(glm::quat& targetRotation, const glm::quat& oldRotation, float mixFraction) { + // the hinge works best without a soft clamp + return clamp(targetRotation); +} + ConeRollerConstraint::ConeRollerConstraint(float coneAngle, const glm::vec3& coneAxis, float minRoll, float maxRoll) : _coneAngle(coneAngle), _minRoll(minRoll), _maxRoll(maxRoll) { assert(_maxRoll >= _minRoll); @@ -117,7 +142,7 @@ ConeRollerConstraint::ConeRollerConstraint(float coneAngle, const glm::vec3& con } // virtual -bool ConeRollerConstraint::applyTo(glm::quat& rotation) const { +bool ConeRollerConstraint::clamp(glm::quat& rotation) const { bool applied = false; glm::vec3 rotatedAxis = rotation * _coneAxis; glm::vec3 perpAxis = glm::cross(rotatedAxis, _coneAxis); @@ -131,8 +156,7 @@ bool ConeRollerConstraint::applyTo(glm::quat& rotation) const { rotatedAxis = rotation * _coneAxis; applied = true; } - } - else { + } else { // the rotation is 100% roll // there is no obvious perp axis so we must pick one perpAxis = rotatedAxis; @@ -168,7 +192,7 @@ bool ConeRollerConstraint::applyTo(glm::quat& rotation) const { float roll = sign * angleBetween(rolledPerpAxis, perpAxis); if (roll < _minRoll || roll > _maxRoll) { float clampedRoll = clampAngle(roll, _minRoll, _maxRoll); - rotation = glm::angleAxis(clampedRoll - roll, rotatedAxis) * rotation; + rotation = glm::normalize(glm::angleAxis(clampedRoll - roll, rotatedAxis) * rotation); applied = true; } return applied; diff --git a/libraries/shared/src/AngularConstraint.h b/libraries/shared/src/AngularConstraint.h index 8003e4c9a3..929a58959b 100644 --- a/libraries/shared/src/AngularConstraint.h +++ b/libraries/shared/src/AngularConstraint.h @@ -24,14 +24,16 @@ public: AngularConstraint() {} virtual ~AngularConstraint() {} - virtual bool applyTo(glm::quat& rotation) const = 0; + virtual bool clamp(glm::quat& rotation) const = 0; + virtual bool softClamp(glm::quat& targetRotation, const glm::quat& oldRotation, float mixFraction); protected: }; class HingeConstraint : public AngularConstraint { public: HingeConstraint(const glm::vec3& forwardAxis, const glm::vec3& rotationAxis, float minAngle, float maxAngle); - virtual bool applyTo(glm::quat& rotation) const; + virtual bool clamp(glm::quat& rotation) const; + virtual bool softClamp(glm::quat& targetRotation, const glm::quat& oldRotation, float mixFraction); protected: glm::vec3 _forwardAxis; glm::vec3 _rotationAxis; @@ -42,7 +44,7 @@ protected: class ConeRollerConstraint : public AngularConstraint { public: ConeRollerConstraint(float coneAngle, const glm::vec3& coneAxis, float minRoll, float maxRoll); - virtual bool applyTo(glm::quat& rotation) const; + virtual bool clamp(glm::quat& rotation) const; private: float _coneAngle; glm::vec3 _coneAxis; diff --git a/tests/shared/src/AngularConstraintTests.cpp b/tests/shared/src/AngularConstraintTests.cpp index d6a9214acc..00916a7267 100644 --- a/tests/shared/src/AngularConstraintTests.cpp +++ b/tests/shared/src/AngularConstraintTests.cpp @@ -36,10 +36,10 @@ void AngularConstraintTests::testHingeConstraint() { glm::quat rotation = glm::angleAxis(angle, yAxis); glm::quat newRotation = rotation; - bool constrained = c->applyTo(newRotation); + bool constrained = c->clamp(newRotation); if (constrained) { std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: HingeConstraint should not applyTo()" << std::endl; + << " ERROR: HingeConstraint should not clamp()" << std::endl; } if (rotation != newRotation) { std::cout << __FILE__ << ":" << __LINE__ @@ -51,10 +51,10 @@ void AngularConstraintTests::testHingeConstraint() { glm::quat rotation = glm::angleAxis(angle, yAxis); glm::quat newRotation = rotation; - bool constrained = c->applyTo(newRotation); + bool constrained = c->clamp(newRotation); if (constrained) { std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: HingeConstraint should not applyTo()" << std::endl; + << " ERROR: HingeConstraint should not clamp()" << std::endl; } if (rotation != newRotation) { std::cout << __FILE__ << ":" << __LINE__ @@ -66,10 +66,10 @@ void AngularConstraintTests::testHingeConstraint() { glm::quat rotation = glm::angleAxis(angle, yAxis); glm::quat newRotation = rotation; - bool constrained = c->applyTo(newRotation); + bool constrained = c->clamp(newRotation); if (constrained) { std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: HingeConstraint should not applyTo()" << std::endl; + << " ERROR: HingeConstraint should not clamp()" << std::endl; } if (rotation != newRotation) { std::cout << __FILE__ << ":" << __LINE__ @@ -81,10 +81,10 @@ void AngularConstraintTests::testHingeConstraint() { glm::quat rotation = glm::angleAxis(angle, yAxis); glm::quat newRotation = rotation; - bool constrained = c->applyTo(newRotation); + bool constrained = c->clamp(newRotation); if (!constrained) { std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: HingeConstraint should applyTo()" << std::endl; + << " ERROR: HingeConstraint should clamp()" << std::endl; } if (rotation == newRotation) { std::cout << __FILE__ << ":" << __LINE__ @@ -102,10 +102,10 @@ void AngularConstraintTests::testHingeConstraint() { glm::quat rotation = glm::angleAxis(angle, yAxis); glm::quat newRotation = rotation; - bool constrained = c->applyTo(newRotation); + bool constrained = c->clamp(newRotation); if (!constrained) { std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: HingeConstraint should applyTo()" << std::endl; + << " ERROR: HingeConstraint should clamp()" << std::endl; } if (rotation == newRotation) { std::cout << __FILE__ << ":" << __LINE__ @@ -123,10 +123,10 @@ void AngularConstraintTests::testHingeConstraint() { glm::quat rotation = glm::angleAxis(angle, yAxis); glm::quat newRotation = rotation; - bool constrained = c->applyTo(newRotation); + bool constrained = c->clamp(newRotation); if (!constrained) { std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: HingeConstraint should applyTo()" << std::endl; + << " ERROR: HingeConstraint should clamp()" << std::endl; } if (rotation == newRotation) { std::cout << __FILE__ << ":" << __LINE__ @@ -144,10 +144,10 @@ void AngularConstraintTests::testHingeConstraint() { glm::quat rotation = glm::angleAxis(angle, yAxis); glm::quat newRotation = rotation; - bool constrained = c->applyTo(newRotation); + bool constrained = c->clamp(newRotation); if (!constrained) { std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: HingeConstraint should applyTo()" << std::endl; + << " ERROR: HingeConstraint should clamp()" << std::endl; } if (rotation == newRotation) { std::cout << __FILE__ << ":" << __LINE__ @@ -169,10 +169,10 @@ void AngularConstraintTests::testHingeConstraint() { glm::quat rotation = offRotation * glm::angleAxis(angle, yAxis); glm::quat newRotation = rotation; - bool constrained = c->applyTo(newRotation); + bool constrained = c->clamp(newRotation); if (!constrained) { std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: HingeConstraint should applyTo()" << std::endl; + << " ERROR: HingeConstraint should clamp()" << std::endl; } if (rotation == newRotation) { std::cout << __FILE__ << ":" << __LINE__ @@ -193,10 +193,10 @@ void AngularConstraintTests::testHingeConstraint() { rotation = offRotation * glm::angleAxis(angle, yAxis); glm::quat newRotation = rotation; - bool constrained = c->applyTo(newRotation); + bool constrained = c->clamp(newRotation); if (!constrained) { std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: HingeConstraint should applyTo()" << std::endl; + << " ERROR: HingeConstraint should clamp()" << std::endl; } if (rotation == newRotation) { std::cout << __FILE__ << ":" << __LINE__ @@ -217,10 +217,10 @@ void AngularConstraintTests::testHingeConstraint() { rotation = offRotation * glm::angleAxis(angle, yAxis); glm::quat newRotation = rotation; - bool constrained = c->applyTo(newRotation); + bool constrained = c->clamp(newRotation); if (!constrained) { std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: HingeConstraint should applyTo()" << std::endl; + << " ERROR: HingeConstraint should clamp()" << std::endl; } if (rotation == newRotation) { std::cout << __FILE__ << ":" << __LINE__ @@ -241,10 +241,10 @@ void AngularConstraintTests::testHingeConstraint() { rotation = offRotation * glm::angleAxis(angle, yAxis); glm::quat newRotation = rotation; - bool constrained = c->applyTo(newRotation); + bool constrained = c->clamp(newRotation); if (!constrained) { std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: HingeConstraint should applyTo()" << std::endl; + << " ERROR: HingeConstraint should clamp()" << std::endl; } if (rotation == newRotation) { std::cout << __FILE__ << ":" << __LINE__ @@ -265,10 +265,10 @@ void AngularConstraintTests::testHingeConstraint() { rotation = offRotation * glm::angleAxis(angle, yAxis); glm::quat newRotation = rotation; - bool constrained = c->applyTo(newRotation); + bool constrained = c->clamp(newRotation); if (!constrained) { std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: HingeConstraint should applyTo()" << std::endl; + << " ERROR: HingeConstraint should clamp()" << std::endl; } if (rotation == newRotation) { std::cout << __FILE__ << ":" << __LINE__ @@ -315,10 +315,10 @@ void AngularConstraintTests::testConeRollerConstraint() { glm::quat rotation(angles); glm::quat newRotation = rotation; - bool constrained = c->applyTo(newRotation); + bool constrained = c->clamp(newRotation); if (constrained) { std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: ConeRollerConstraint should not applyTo()" << std::endl; + << " ERROR: ConeRollerConstraint should not clamp()" << std::endl; } if (rotation != newRotation) { std::cout << __FILE__ << ":" << __LINE__ @@ -330,10 +330,10 @@ void AngularConstraintTests::testConeRollerConstraint() { glm::quat rotation = glm::angleAxis(expectedConeAngle - deltaAngle, perpAxis); glm::quat newRotation = rotation; - bool constrained = c->applyTo(newRotation); + bool constrained = c->clamp(newRotation); if (constrained) { std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: ConeRollerConstraint should not applyTo()" << std::endl; + << " ERROR: ConeRollerConstraint should not clamp()" << std::endl; } if (rotation != newRotation) { std::cout << __FILE__ << ":" << __LINE__ @@ -344,10 +344,10 @@ void AngularConstraintTests::testConeRollerConstraint() { glm::quat rotation = glm::angleAxis(expectedConeAngle + deltaAngle, perpAxis); glm::quat newRotation = rotation; - bool constrained = c->applyTo(newRotation); + bool constrained = c->clamp(newRotation); if (!constrained) { std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: ConeRollerConstraint should applyTo()" << std::endl; + << " ERROR: ConeRollerConstraint should clamp()" << std::endl; } if (rotation == newRotation) { std::cout << __FILE__ << ":" << __LINE__ @@ -358,10 +358,10 @@ void AngularConstraintTests::testConeRollerConstraint() { glm::quat rotation = glm::angleAxis(minAngleZ + deltaAngle, expectedConeAxis); glm::quat newRotation = rotation; - bool constrained = c->applyTo(newRotation); + bool constrained = c->clamp(newRotation); if (constrained) { std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: ConeRollerConstraint should not applyTo()" << std::endl; + << " ERROR: ConeRollerConstraint should not clamp()" << std::endl; } if (rotation != newRotation) { std::cout << __FILE__ << ":" << __LINE__ @@ -372,10 +372,10 @@ void AngularConstraintTests::testConeRollerConstraint() { glm::quat rotation = glm::angleAxis(maxAngleZ - deltaAngle, expectedConeAxis); glm::quat newRotation = rotation; - bool constrained = c->applyTo(newRotation); + bool constrained = c->clamp(newRotation); if (constrained) { std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: ConeRollerConstraint should not applyTo()" << std::endl; + << " ERROR: ConeRollerConstraint should not clamp()" << std::endl; } if (rotation != newRotation) { std::cout << __FILE__ << ":" << __LINE__ @@ -386,10 +386,10 @@ void AngularConstraintTests::testConeRollerConstraint() { glm::quat rotation = glm::angleAxis(minAngleZ - deltaAngle, expectedConeAxis); glm::quat newRotation = rotation; - bool constrained = c->applyTo(newRotation); + bool constrained = c->clamp(newRotation); if (!constrained) { std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: ConeRollerConstraint should applyTo()" << std::endl; + << " ERROR: ConeRollerConstraint should clamp()" << std::endl; } if (rotation == newRotation) { std::cout << __FILE__ << ":" << __LINE__ @@ -405,10 +405,10 @@ void AngularConstraintTests::testConeRollerConstraint() { glm::quat rotation = glm::angleAxis(maxAngleZ + deltaAngle, expectedConeAxis); glm::quat newRotation = rotation; - bool constrained = c->applyTo(newRotation); + bool constrained = c->clamp(newRotation); if (!constrained) { std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: ConeRollerConstraint should applyTo()" << std::endl; + << " ERROR: ConeRollerConstraint should clamp()" << std::endl; } if (rotation == newRotation) { std::cout << __FILE__ << ":" << __LINE__ @@ -427,10 +427,10 @@ void AngularConstraintTests::testConeRollerConstraint() { glm::quat rotation = pitchYaw * roll; glm::quat newRotation = rotation; - bool constrained = c->applyTo(newRotation); + bool constrained = c->clamp(newRotation); if (!constrained) { std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: ConeRollerConstraint should applyTo()" << std::endl; + << " ERROR: ConeRollerConstraint should clamp()" << std::endl; } if (rotation == newRotation) { std::cout << __FILE__ << ":" << __LINE__ @@ -450,10 +450,10 @@ void AngularConstraintTests::testConeRollerConstraint() { glm::quat rotation = pitchYaw * roll; glm::quat newRotation = rotation; - bool constrained = c->applyTo(newRotation); + bool constrained = c->clamp(newRotation); if (!constrained) { std::cout << __FILE__ << ":" << __LINE__ - << " ERROR: ConeRollerConstraint should applyTo()" << std::endl; + << " ERROR: ConeRollerConstraint should clamp()" << std::endl; } if (rotation == newRotation) { std::cout << __FILE__ << ":" << __LINE__