From 1b24a111fe28869376fd3683daa8d32db5d38cf3 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 25 Jun 2014 09:56:18 -0700 Subject: [PATCH 1/3] JointState::_rotationInParentFrame is now private --- interface/src/avatar/FaceModel.cpp | 8 ++++---- interface/src/avatar/SkeletonModel.cpp | 6 +++--- interface/src/renderer/JointState.cpp | 16 ++++++++-------- interface/src/renderer/JointState.h | 16 +++++++++++----- interface/src/renderer/Model.cpp | 8 ++++---- 5 files changed, 30 insertions(+), 24 deletions(-) diff --git a/interface/src/avatar/FaceModel.cpp b/interface/src/avatar/FaceModel.cpp index c3f31ff6e7..59e5b08cc0 100644 --- a/interface/src/avatar/FaceModel.cpp +++ b/interface/src/avatar/FaceModel.cpp @@ -51,10 +51,10 @@ void FaceModel::maybeUpdateNeckRotation(const JointState& parentState, const FBX glm::mat3 axes = glm::mat3_cast(glm::quat()); glm::mat3 inverse = glm::mat3(glm::inverse(parentState.getTransform() * glm::translate(state.getDefaultTranslationInParentFrame()) * joint.preTransform * glm::mat4_cast(joint.preRotation))); - state._rotationInParentFrame = glm::angleAxis(- RADIANS_PER_DEGREE * _owningHead->getFinalRoll(), glm::normalize(inverse * axes[2])) + state.setRotationInParentFrame(glm::angleAxis(- RADIANS_PER_DEGREE * _owningHead->getFinalRoll(), glm::normalize(inverse * axes[2])) * glm::angleAxis(RADIANS_PER_DEGREE * _owningHead->getFinalYaw(), glm::normalize(inverse * axes[1])) * glm::angleAxis(- RADIANS_PER_DEGREE * _owningHead->getFinalPitch(), glm::normalize(inverse * axes[0])) - * joint.rotation; + * joint.rotation); } void FaceModel::maybeUpdateEyeRotation(const JointState& parentState, const FBXJoint& joint, JointState& state) { @@ -68,8 +68,8 @@ void FaceModel::maybeUpdateEyeRotation(const JointState& parentState, const FBXJ _owningHead->getSaccade() - _translation, 1.0f)); glm::quat between = rotationBetween(front, lookAt); const float MAX_ANGLE = 30.0f * RADIANS_PER_DEGREE; - state._rotationInParentFrame = glm::angleAxis(glm::clamp(glm::angle(between), -MAX_ANGLE, MAX_ANGLE), glm::axis(between)) * - joint.rotation; + state.setRotationInParentFrame(glm::angleAxis(glm::clamp(glm::angle(between), -MAX_ANGLE, MAX_ANGLE), glm::axis(between)) * + joint.rotation); } void FaceModel::updateJointState(int index) { diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index fdb3ce03d8..bdda30c9a3 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -220,7 +220,7 @@ void SkeletonModel::applyPalmData(int jointIndex, PalmData& palm) { JointState& parentState = _jointStates[parentJointIndex]; parentState.setRotationFromBindFrame(palmRotation, PALM_PRIORITY); // lock hand to forearm by slamming its rotation (in parent-frame) to identity - _jointStates[jointIndex]._rotationInParentFrame = glm::quat(); + _jointStates[jointIndex].setRotationInParentFrame(glm::quat()); } else { setJointPosition(jointIndex, palmPosition, palmRotation, true, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY); @@ -259,9 +259,9 @@ void SkeletonModel::maybeUpdateLeanRotation(const JointState& parentState, const glm::mat3 axes = glm::mat3_cast(glm::quat()); glm::mat3 inverse = glm::mat3(glm::inverse(parentState.getTransform() * glm::translate(state.getDefaultTranslationInParentFrame()) * joint.preTransform * glm::mat4_cast(joint.preRotation * joint.rotation))); - state._rotationInParentFrame = glm::angleAxis(- RADIANS_PER_DEGREE * _owningAvatar->getHead()->getFinalLeanSideways(), + state.setRotationInParentFrame(glm::angleAxis(- RADIANS_PER_DEGREE * _owningAvatar->getHead()->getFinalLeanSideways(), glm::normalize(inverse * axes[2])) * glm::angleAxis(- RADIANS_PER_DEGREE * _owningAvatar->getHead()->getFinalLeanForward(), - glm::normalize(inverse * axes[0])) * joint.rotation; + glm::normalize(inverse * axes[0])) * joint.rotation); } void SkeletonModel::maybeUpdateNeckRotation(const JointState& parentState, const FBXJoint& joint, JointState& state) { diff --git a/interface/src/renderer/JointState.cpp b/interface/src/renderer/JointState.cpp index e66a2f44e9..9219db8840 100644 --- a/interface/src/renderer/JointState.cpp +++ b/interface/src/renderer/JointState.cpp @@ -21,6 +21,14 @@ JointState::JointState() : _fbxJoint(NULL) { } +void JointState::copyState(const JointState& state) { + _animationPriority = state._animationPriority; + _transform = state._transform; + _rotation = extractRotation(_transform); + _rotationInParentFrame = state._rotationInParentFrame; + // DO NOT copy _fbxJoint +} + void JointState::setFBXJoint(const FBXJoint* joint) { assert(joint != NULL); _rotationInParentFrame = joint->rotation; @@ -28,14 +36,6 @@ void JointState::setFBXJoint(const FBXJoint* joint) { _fbxJoint = joint; } -void JointState::copyState(const JointState& state) { - _rotationInParentFrame = state._rotationInParentFrame; - _transform = state._transform; - _rotation = extractRotation(_transform); - _animationPriority = state._animationPriority; - // DO NOT copy _fbxJoint -} - void JointState::computeTransform(const glm::mat4& parentTransform) { glm::quat modifiedRotation = _fbxJoint->preRotation * _rotationInParentFrame * _fbxJoint->postRotation; glm::mat4 modifiedTransform = _fbxJoint->preTransform * glm::mat4_cast(modifiedRotation) * _fbxJoint->postTransform; diff --git a/interface/src/renderer/JointState.h b/interface/src/renderer/JointState.h index b1a584d4ec..81023f89ca 100644 --- a/interface/src/renderer/JointState.h +++ b/interface/src/renderer/JointState.h @@ -22,12 +22,14 @@ class JointState { public: JointState(); + void copyState(const JointState& state); + void setFBXJoint(const FBXJoint* joint); const FBXJoint& getFBXJoint() const { return *_fbxJoint; } - void copyState(const JointState& state); - void computeTransform(const glm::mat4& parentTransform); + + const glm::mat4& getTransform() const { return _transform; } glm::quat getRotation() const { return _rotation; } @@ -42,8 +44,6 @@ public: /// \param delta is in the jointParent-frame void applyRotationDelta(const glm::quat& delta, bool constrain = true, float priority = 1.0f); - const glm::vec3& getDefaultTranslationInParentFrame() const; - void restoreRotation(float fraction, float priority); /// \param rotation is from bind- to model-frame @@ -51,14 +51,20 @@ public: /// NOTE: the JointState's model-frame transform/rotation are NOT updated! void setRotationFromBindFrame(const glm::quat& rotation, float priority); + void setRotationInParentFrame(const glm::quat& rotation) { _rotationInParentFrame = rotation; } + const glm::quat& getRotationInParentFrame() const { return _rotationInParentFrame; } + + const glm::vec3& getDefaultTranslationInParentFrame() const; + + void clearTransformTranslation(); - glm::quat _rotationInParentFrame; // joint- to parentJoint-frame float _animationPriority; // the priority of the animation affecting this joint private: glm::mat4 _transform; // joint- to model-frame glm::quat _rotation; // joint- to model-frame + glm::quat _rotationInParentFrame; // joint- to parentJoint-frame const FBXJoint* _fbxJoint; // JointState does NOT own its FBXJoint }; diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp index 3b5cda4fd2..7afcb2a736 100644 --- a/interface/src/renderer/Model.cpp +++ b/interface/src/renderer/Model.cpp @@ -460,7 +460,7 @@ void Model::reset() { } const FBXGeometry& geometry = _geometry->getFBXGeometry(); for (int i = 0; i < _jointStates.size(); i++) { - _jointStates[i]._rotationInParentFrame = geometry.joints.at(i).rotation; + _jointStates[i].setRotationInParentFrame(geometry.joints.at(i).rotation); } } @@ -686,7 +686,7 @@ bool Model::getJointState(int index, glm::quat& rotation) const { if (index == -1 || index >= _jointStates.size()) { return false; } - rotation = _jointStates.at(index)._rotationInParentFrame; + rotation = _jointStates.at(index).getRotationInParentFrame(); const glm::quat& defaultRotation = _geometry->getFBXGeometry().joints.at(index).rotation; return glm::abs(rotation.x - defaultRotation.x) >= EPSILON || glm::abs(rotation.y - defaultRotation.y) >= EPSILON || @@ -699,7 +699,7 @@ void Model::setJointState(int index, bool valid, const glm::quat& rotation, floa JointState& state = _jointStates[index]; if (priority >= state._animationPriority) { if (valid) { - state._rotationInParentFrame = rotation; + state.setRotationInParentFrame(rotation); state._animationPriority = priority; } else { state.restoreRotation(1.0f, priority); @@ -1605,7 +1605,7 @@ void AnimationHandle::applyFrame(float frameIndex) { if (mapping != -1) { JointState& state = _model->_jointStates[mapping]; if (_priority >= state._animationPriority) { - state._rotationInParentFrame = safeMix(floorFrame.rotations.at(i), ceilFrame.rotations.at(i), frameFraction); + state.setRotationInParentFrame(safeMix(floorFrame.rotations.at(i), ceilFrame.rotations.at(i), frameFraction)); state._animationPriority = _priority; } } From c12c869cdfb981bc22ffd662959b5e0b5b99892d Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 25 Jun 2014 18:06:56 -0700 Subject: [PATCH 2/3] add "visible" transforms to JointState also stubbery for using them in Model and SkeletonModel --- interface/src/avatar/SkeletonModel.cpp | 7 +++- interface/src/avatar/SkeletonModel.h | 4 +- interface/src/renderer/JointState.cpp | 21 +++++++++- interface/src/renderer/JointState.h | 11 ++++++ interface/src/renderer/Model.cpp | 54 +++++++++++++++++++++++--- interface/src/renderer/Model.h | 6 +++ 6 files changed, 94 insertions(+), 9 deletions(-) diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index bdda30c9a3..0229d686d4 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -539,6 +539,11 @@ void SkeletonModel::buildRagdollConstraints() { } } +void SkeletonModel::updateVisibleJointStates() { + Model::updateVisibleJointStates(); + // TODO: implement this to move visible joints to agree with joint shape positions +} + // virtual void SkeletonModel::stepRagdollForward(float deltaTime) { const float RAGDOLL_FOLLOWS_JOINTS_TIMESCALE = 0.03f; @@ -699,7 +704,7 @@ void SkeletonModel::computeBoundingShape(const FBXGeometry& geometry) { _boundingRadius = 0.5f * glm::length(diagonal); } -void SkeletonModel::resetShapePositions() { +void SkeletonModel::resetShapePositionsToDefaultPose() { // DEBUG method. // Moves shapes to the joint default locations for debug visibility into // how the bounding shape is computed. diff --git a/interface/src/avatar/SkeletonModel.h b/interface/src/avatar/SkeletonModel.h index b91c112b6a..76d0d45efa 100644 --- a/interface/src/avatar/SkeletonModel.h +++ b/interface/src/avatar/SkeletonModel.h @@ -93,6 +93,8 @@ public: /// Retrieve the positions of up to two eye meshes. /// \return whether or not both eye meshes were found bool getEyePositions(glm::vec3& firstEyePosition, glm::vec3& secondEyePosition) const; + + virtual void updateVisibleJointStates(); // virtual overrride from Ragdoll virtual void stepRagdollForward(float deltaTime); @@ -104,7 +106,7 @@ public: float getBoundingShapeRadius() const { return _boundingShape.getRadius(); } const CapsuleShape& getBoundingShape() const { return _boundingShape; } - void resetShapePositions(); // DEBUG method + void resetShapePositionsToDefaultPose(); // DEBUG method void renderRagdoll(); protected: diff --git a/interface/src/renderer/JointState.cpp b/interface/src/renderer/JointState.cpp index 9219db8840..f75bbe8bec 100644 --- a/interface/src/renderer/JointState.cpp +++ b/interface/src/renderer/JointState.cpp @@ -11,7 +11,6 @@ #include -//#include #include #include "JointState.h" @@ -26,6 +25,10 @@ void JointState::copyState(const JointState& state) { _transform = state._transform; _rotation = extractRotation(_transform); _rotationInParentFrame = state._rotationInParentFrame; + + _visibleTransform = state._visibleTransform; + _visibleRotation = extractRotation(_visibleTransform); + _visibleRotationInParentFrame = state._visibleRotationInParentFrame; // DO NOT copy _fbxJoint } @@ -43,6 +46,13 @@ void JointState::computeTransform(const glm::mat4& parentTransform) { _rotation = extractRotation(_transform); } +void JointState::computeVisibleTransform(const glm::mat4& parentTransform) { + glm::quat modifiedRotation = _fbxJoint->preRotation * _visibleRotationInParentFrame * _fbxJoint->postRotation; + glm::mat4 modifiedTransform = _fbxJoint->preTransform * glm::mat4_cast(modifiedRotation) * _fbxJoint->postTransform; + _visibleTransform = parentTransform * glm::translate(_fbxJoint->translation) * modifiedTransform; + _visibleRotation = extractRotation(_visibleTransform); +} + glm::quat JointState::getRotationFromBindToModelFrame() const { return _rotation * _fbxJoint->inverseBindRotation; } @@ -68,6 +78,9 @@ void JointState::clearTransformTranslation() { _transform[3][0] = 0.0f; _transform[3][1] = 0.0f; _transform[3][2] = 0.0f; + _visibleTransform[3][0] = 0.0f; + _visibleTransform[3][1] = 0.0f; + _visibleTransform[3][2] = 0.0f; } void JointState::setRotation(const glm::quat& rotation, bool constrain, float priority) { @@ -99,3 +112,9 @@ const glm::vec3& JointState::getDefaultTranslationInParentFrame() const { assert(_fbxJoint != NULL); return _fbxJoint->translation; } + +void JointState::slaveVisibleTransform() { + _visibleTransform = _transform; + _visibleRotation = _rotation; + _visibleRotationInParentFrame = _rotationInParentFrame; +} diff --git a/interface/src/renderer/JointState.h b/interface/src/renderer/JointState.h index 81023f89ca..b42132d0a0 100644 --- a/interface/src/renderer/JointState.h +++ b/interface/src/renderer/JointState.h @@ -27,8 +27,13 @@ public: void setFBXJoint(const FBXJoint* joint); const FBXJoint& getFBXJoint() const { return *_fbxJoint; } + void computeTransform(const glm::mat4& parentTransform); + void computeVisibleTransform(const glm::mat4& parentTransform); + const glm::mat4& getVisibleTransform() const { return _visibleTransform; } + glm::quat getVisibleRotation() const { return _visibleRotation; } + glm::vec3 getVisiblePosition() const { return extractTranslation(_visibleTransform); } const glm::mat4& getTransform() const { return _transform; } @@ -59,6 +64,8 @@ public: void clearTransformTranslation(); + void slaveVisibleTransform(); + float _animationPriority; // the priority of the animation affecting this joint private: @@ -66,6 +73,10 @@ private: glm::quat _rotation; // joint- to model-frame glm::quat _rotationInParentFrame; // joint- to parentJoint-frame + glm::mat4 _visibleTransform; + glm::quat _visibleRotation; + glm::quat _visibleRotationInParentFrame; + const FBXJoint* _fbxJoint; // JointState does NOT own its FBXJoint }; diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp index 7afcb2a736..5d82b111b7 100644 --- a/interface/src/renderer/Model.cpp +++ b/interface/src/renderer/Model.cpp @@ -39,8 +39,8 @@ Model::Model(QObject* parent) : _scaledToFit(false), _snapModelToCenter(false), _snappedToCenter(false), + _showTrueJointTransforms(false), _rootIndex(-1), - //_enableCollisionShapes(false), _lodDistance(0.0f), _pupilDilation(0.0f), _url("http://invalid.com") { @@ -571,6 +571,9 @@ void Model::setJointStates(QVector states) { radius = distance; } } + for (int i = 0; i < _jointStates.size(); i++) { + _jointStates[i].slaveVisibleTransform(); + } _boundingRadius = radius; } @@ -765,6 +768,23 @@ bool Model::getJointCombinedRotation(int jointIndex, glm::quat& rotation) const return true; } +bool Model::getVisibleJointPositionInWorldFrame(int jointIndex, glm::vec3& position) const { + if (jointIndex == -1 || jointIndex >= _jointStates.size()) { + return false; + } + // position is in world-frame + position = _translation + _rotation * _jointStates[jointIndex].getVisiblePosition(); + return true; +} + +bool Model::getVisibleJointRotationInWorldFrame(int jointIndex, glm::quat& rotation) const { + if (jointIndex == -1 || jointIndex >= _jointStates.size()) { + return false; + } + rotation = _rotation * _jointStates[jointIndex].getVisibleRotation(); + return true; +} + QStringList Model::getJointNames() const { if (QThread::currentThread() != thread()) { QStringList result; @@ -918,6 +938,8 @@ void Model::simulateInternal(float deltaTime) { for (int i = 0; i < _jointStates.size(); i++) { updateJointState(i); } + updateVisibleJointStates(); + _shapesAreDirty = ! _shapes.isEmpty(); // update the attachment transforms and simulate them @@ -928,8 +950,13 @@ void Model::simulateInternal(float deltaTime) { glm::vec3 jointTranslation = _translation; glm::quat jointRotation = _rotation; - getJointPositionInWorldFrame(attachment.jointIndex, jointTranslation); - getJointRotationInWorldFrame(attachment.jointIndex, jointRotation); + if (_showTrueJointTransforms) { + getJointPositionInWorldFrame(attachment.jointIndex, jointTranslation); + getJointRotationInWorldFrame(attachment.jointIndex, jointRotation); + } else { + getVisibleJointPositionInWorldFrame(attachment.jointIndex, jointTranslation); + getVisibleJointRotationInWorldFrame(attachment.jointIndex, jointRotation); + } model->setTranslation(jointTranslation + jointRotation * attachment.translation * _scale); model->setRotation(jointRotation * attachment.rotation); @@ -944,9 +971,16 @@ void Model::simulateInternal(float deltaTime) { for (int i = 0; i < _meshStates.size(); i++) { MeshState& state = _meshStates[i]; const FBXMesh& mesh = geometry.meshes.at(i); - for (int j = 0; j < mesh.clusters.size(); j++) { - const FBXCluster& cluster = mesh.clusters.at(j); - state.clusterMatrices[j] = modelToWorld * _jointStates[cluster.jointIndex].getTransform() * cluster.inverseBindMatrix; + if (_showTrueJointTransforms) { + for (int j = 0; j < mesh.clusters.size(); j++) { + const FBXCluster& cluster = mesh.clusters.at(j); + state.clusterMatrices[j] = modelToWorld * _jointStates[cluster.jointIndex].getTransform() * cluster.inverseBindMatrix; + } + } else { + for (int j = 0; j < mesh.clusters.size(); j++) { + const FBXCluster& cluster = mesh.clusters.at(j); + state.clusterMatrices[j] = modelToWorld * _jointStates[cluster.jointIndex].getVisibleTransform() * cluster.inverseBindMatrix; + } } } @@ -972,6 +1006,14 @@ void Model::updateJointState(int index) { } } +void Model::updateVisibleJointStates() { + if (!_showTrueJointTransforms) { + for (int i = 0; i < _jointStates.size(); i++) { + _jointStates[i].slaveVisibleTransform(); + } + } +} + bool Model::setJointPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation, bool useRotation, int lastFreeIndex, bool allIntermediatesFree, const glm::vec3& alignment, float priority) { if (jointIndex == -1 || _jointStates.isEmpty()) { diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h index 2045a0c9b5..8572831d8b 100644 --- a/interface/src/renderer/Model.h +++ b/interface/src/renderer/Model.h @@ -120,6 +120,9 @@ public: bool getJointRotationInWorldFrame(int jointIndex, glm::quat& rotation) const; bool getJointCombinedRotation(int jointIndex, glm::quat& rotation) const; + bool getVisibleJointPositionInWorldFrame(int jointIndex, glm::vec3& position) const; + bool getVisibleJointRotationInWorldFrame(int jointIndex, glm::quat& rotation) const; + /// \param jointIndex index of joint in model structure /// \param position[out] position of joint in model-frame /// \return true if joint exists @@ -152,6 +155,7 @@ protected: bool _snapModelToCenter; /// is the model's offset automatically adjusted to center around 0,0,0 in model space bool _snappedToCenter; /// are we currently snapped to center + bool _showTrueJointTransforms; int _rootIndex; QVector _jointStates; @@ -176,6 +180,8 @@ protected: /// Updates the state of the joint at the specified index. virtual void updateJointState(int index); + + virtual void updateVisibleJointStates(); /// \param jointIndex index of joint in model structure /// \param position position of joint in model-frame From 49d54dd6f0d900e92325006ab53620cebee16fb2 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 2 Jul 2014 09:33:51 -0700 Subject: [PATCH 3/3] Add Model::inverseKinematics() for stable hand IK (removed constraint enforcement which was causing problems, will add that back later) --- interface/src/avatar/SkeletonModel.cpp | 15 +-- interface/src/renderer/JointState.cpp | 55 ++++++++--- interface/src/renderer/JointState.h | 22 ++++- interface/src/renderer/Model.cpp | 122 +++++++++++++++++++++++++ interface/src/renderer/Model.h | 2 + 5 files changed, 195 insertions(+), 21 deletions(-) diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp index 0229d686d4..e5751360b5 100644 --- a/interface/src/avatar/SkeletonModel.cpp +++ b/interface/src/avatar/SkeletonModel.cpp @@ -222,8 +222,7 @@ void SkeletonModel::applyPalmData(int jointIndex, PalmData& palm) { // lock hand to forearm by slamming its rotation (in parent-frame) to identity _jointStates[jointIndex].setRotationInParentFrame(glm::quat()); } else { - setJointPosition(jointIndex, palmPosition, palmRotation, - true, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY); + inverseKinematics(jointIndex, palmPosition, palmRotation, PALM_PRIORITY); } } @@ -642,13 +641,15 @@ void SkeletonModel::computeBoundingShape(const FBXGeometry& geometry) { // compute the default transforms and slam the ragdoll positions accordingly // (which puts the shapes where we want them) - transforms[0] = _jointStates[0].getTransform(); - _ragdollPoints[0]._position = extractTranslation(transforms[0]); - _ragdollPoints[0]._lastPosition = _ragdollPoints[0]._position; - for (int i = 1; i < numJoints; i++) { + for (int i = 0; i < numJoints; i++) { const FBXJoint& joint = geometry.joints.at(i); int parentIndex = joint.parentIndex; - assert(parentIndex != -1); + if (parentIndex == -1) { + transforms[i] = _jointStates[i].getTransform(); + _ragdollPoints[i]._position = extractTranslation(transforms[i]); + _ragdollPoints[i]._lastPosition = _ragdollPoints[i]._position; + continue; + } glm::quat modifiedRotation = joint.preRotation * joint.rotation * joint.postRotation; transforms[i] = transforms[parentIndex] * glm::translate(joint.translation) diff --git a/interface/src/renderer/JointState.cpp b/interface/src/renderer/JointState.cpp index f75bbe8bec..8e4aecdf51 100644 --- a/interface/src/renderer/JointState.cpp +++ b/interface/src/renderer/JointState.cpp @@ -17,7 +17,8 @@ JointState::JointState() : _animationPriority(0.0f), - _fbxJoint(NULL) { + _fbxJoint(NULL), + _isConstrained(false) { } void JointState::copyState(const JointState& state) { @@ -37,6 +38,10 @@ void JointState::setFBXJoint(const FBXJoint* joint) { _rotationInParentFrame = joint->rotation; // NOTE: JointState does not own the FBXJoint to which it points. _fbxJoint = joint; + // precompute whether there are any constraints or not + float distanceMin = glm::distance(_fbxJoint->rotationMin, glm::vec3(-PI)); + float distanceMax = glm::distance(_fbxJoint->rotationMax, glm::vec3(PI)); + _isConstrained = distanceMin > EPSILON || distanceMax > EPSILON; } void JointState::computeTransform(const glm::mat4& parentTransform) { @@ -60,16 +65,16 @@ glm::quat JointState::getRotationFromBindToModelFrame() const { void JointState::restoreRotation(float fraction, float priority) { assert(_fbxJoint != NULL); if (priority == _animationPriority || _animationPriority == 0.0f) { - _rotationInParentFrame = safeMix(_rotationInParentFrame, _fbxJoint->rotation, fraction); + setRotationInParentFrame(safeMix(_rotationInParentFrame, _fbxJoint->rotation, fraction)); _animationPriority = 0.0f; } } void JointState::setRotationFromBindFrame(const glm::quat& rotation, float priority) { + // rotation is from bind- to model-frame assert(_fbxJoint != NULL); if (priority >= _animationPriority) { - // rotation is from bind- to model-frame - _rotationInParentFrame = _rotationInParentFrame * glm::inverse(_rotation) * rotation * glm::inverse(_fbxJoint->inverseBindRotation); + setRotationInParentFrame(_rotationInParentFrame * glm::inverse(_rotation) * rotation * glm::inverse(_fbxJoint->inverseBindRotation)); _animationPriority = priority; } } @@ -88,24 +93,50 @@ void JointState::setRotation(const glm::quat& rotation, bool constrain, float pr } void JointState::applyRotationDelta(const glm::quat& delta, bool constrain, float priority) { - // NOTE: delta is in jointParent-frame + // NOTE: delta is in model-frame assert(_fbxJoint != NULL); if (priority < _animationPriority) { return; } _animationPriority = priority; - if (!constrain || (_fbxJoint->rotationMin == glm::vec3(-PI, -PI, -PI) && - _fbxJoint->rotationMax == glm::vec3(PI, PI, PI))) { + if (!constrain || !_isConstrained) { // no constraints _rotationInParentFrame = _rotationInParentFrame * glm::inverse(_rotation) * delta * _rotation; _rotation = delta * _rotation; return; } - glm::quat targetRotation = delta * _rotation; - glm::vec3 eulers = safeEulerAngles(_rotationInParentFrame * glm::inverse(_rotation) * targetRotation); - glm::quat newRotation = glm::quat(glm::clamp(eulers, _fbxJoint->rotationMin, _fbxJoint->rotationMax)); - _rotation = _rotation * glm::inverse(_rotationInParentFrame) * newRotation; - _rotationInParentFrame = newRotation; + glm::quat targetRotation = _rotationInParentFrame * glm::inverse(_rotation) * delta * _rotation; + setRotationInParentFrame(targetRotation); +} + +/// Applies delta rotation to joint but mixes a little bit of the default pose as well. +/// This helps keep an IK solution stable. +void JointState::mixRotationDelta(const glm::quat& delta, float mixFactor, float priority) { + // NOTE: delta is in model-frame + assert(_fbxJoint != NULL); + if (priority < _animationPriority) { + return; + } + _animationPriority = priority; + glm::quat targetRotation = _rotationInParentFrame * glm::inverse(_rotation) * delta * _rotation; + if (mixFactor > 0.0f && mixFactor <= 1.0f) { + targetRotation = safeMix(targetRotation, _fbxJoint->rotation, mixFactor); + } + setRotationInParentFrame(targetRotation); +} + + +glm::quat JointState::computeParentRotation() const { + // R = Rp * Rpre * r * Rpost + // Rp = R * (Rpre * r * Rpost)^ + return _rotation * glm::inverse(_fbxJoint->preRotation * _rotationInParentFrame * _fbxJoint->postRotation); +} + +void JointState::setRotationInParentFrame(const glm::quat& targetRotation) { + glm::quat parentRotation = computeParentRotation(); + _rotationInParentFrame = targetRotation; + // R' = Rp * Rpre * r' * Rpost + _rotation = parentRotation * _fbxJoint->preRotation * _rotationInParentFrame * _fbxJoint->postRotation; } const glm::vec3& JointState::getDefaultTranslationInParentFrame() const { diff --git a/interface/src/renderer/JointState.h b/interface/src/renderer/JointState.h index b42132d0a0..8412cfd0cb 100644 --- a/interface/src/renderer/JointState.h +++ b/interface/src/renderer/JointState.h @@ -46,9 +46,19 @@ public: /// \param rotation rotation of joint in model-frame void setRotation(const glm::quat& rotation, bool constrain, float priority); - /// \param delta is in the jointParent-frame + /// \param delta is in the model-frame void applyRotationDelta(const glm::quat& delta, bool constrain = true, float priority = 1.0f); + /// Applies delta rotation to joint but mixes a little bit of the default pose as well. + /// This helps keep an IK solution stable. + /// \param delta rotation change in model-frame + /// \param mixFactor fraction in range [0,1] of how much default pose to blend in (0 is none, 1 is all) + /// \param priority priority level of this animation blend + void mixRotationDelta(const glm::quat& delta, float mixFactor, float priority = 1.0f); + + /// Blends a fraciton of default pose into joint rotation. + /// \param fraction fraction in range [0,1] of how much default pose to blend in (0 is none, 1 is all) + /// \param priority priority level of this animation blend void restoreRotation(float fraction, float priority); /// \param rotation is from bind- to model-frame @@ -56,7 +66,7 @@ public: /// NOTE: the JointState's model-frame transform/rotation are NOT updated! void setRotationFromBindFrame(const glm::quat& rotation, float priority); - void setRotationInParentFrame(const glm::quat& rotation) { _rotationInParentFrame = rotation; } + void setRotationInParentFrame(const glm::quat& targetRotation); const glm::quat& getRotationInParentFrame() const { return _rotationInParentFrame; } const glm::vec3& getDefaultTranslationInParentFrame() const; @@ -69,6 +79,13 @@ public: float _animationPriority; // the priority of the animation affecting this joint private: + /// \return parent model-frame rotation + // (used to keep _rotation consistent when modifying _rotationInWorldFrame directly) + glm::quat computeParentRotation() const; + + /// debug helper function + void loadBindRotation(); + glm::mat4 _transform; // joint- to model-frame glm::quat _rotation; // joint- to model-frame glm::quat _rotationInParentFrame; // joint- to parentJoint-frame @@ -78,6 +95,7 @@ private: glm::quat _visibleRotationInParentFrame; const FBXJoint* _fbxJoint; // JointState does NOT own its FBXJoint + bool _isConstrained; }; #endif // hifi_JointState_h diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp index 5d82b111b7..ffca2c8ea6 100644 --- a/interface/src/renderer/Model.cpp +++ b/interface/src/renderer/Model.cpp @@ -1100,6 +1100,128 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& position, const gl return true; } +void Model::inverseKinematics(int endIndex, glm::vec3 targetPosition, const glm::quat& targetRotation, float priority) { + // NOTE: targetRotation is from bind- to model-frame + + if (endIndex == -1 || _jointStates.isEmpty()) { + return; + } + + const FBXGeometry& geometry = _geometry->getFBXGeometry(); + const QVector& freeLineage = geometry.joints.at(endIndex).freeLineage; + if (freeLineage.isEmpty()) { + return; + } + int numFree = freeLineage.size(); + + // store and remember topmost parent transform + glm::mat4 topParentTransform; + { + int index = freeLineage.last(); + const JointState& state = _jointStates.at(index); + const FBXJoint& joint = state.getFBXJoint(); + int parentIndex = joint.parentIndex; + if (parentIndex == -1) { + const FBXGeometry& geometry = _geometry->getFBXGeometry(); + topParentTransform = glm::scale(_scale) * glm::translate(_offset) * geometry.offset; + } else { + topParentTransform = _jointStates[parentIndex].getTransform(); + } + } + + // this is a cyclic coordinate descent algorithm: see + // http://www.ryanjuckett.com/programming/animation/21-cyclic-coordinate-descent-in-2d + + // keep track of the position of the end-effector + JointState& endState = _jointStates[endIndex]; + glm::vec3 endPosition = endState.getPosition(); + float distanceToGo = glm::distance(targetPosition, endPosition); + + const int MAX_ITERATION_COUNT = 2; + const float ACCEPTABLE_IK_ERROR = 0.005f; // 5mm + int numIterations = 0; + do { + ++numIterations; + // moving up, rotate each free joint to get endPosition closer to target + for (int j = 1; j < numFree; j++) { + int nextIndex = freeLineage.at(j); + JointState& nextState = _jointStates[nextIndex]; + FBXJoint nextJoint = nextState.getFBXJoint(); + if (! nextJoint.isFree) { + continue; + } + + glm::vec3 pivot = nextState.getPosition(); + glm::vec3 leverArm = endPosition - pivot; + float leverLength = glm::length(leverArm); + if (leverLength < EPSILON) { + continue; + } + glm::quat deltaRotation = rotationBetween(leverArm, targetPosition - pivot); + + /* DON'T REMOVE! This code provides the gravitational effect on the IK solution. + * It is commented out for the moment because we're blending the IK solution with + * the default pose which provides similar stability, but we might want to use + * gravity again later. + + // We want to mix the shortest rotation with one that will pull the system down with gravity. + // So we compute a simplified center of mass, where each joint has a mass of 1.0 and we don't + // bother averaging it because we only need direction. + if (j > 1) { + + glm::vec3 centerOfMass(0.0f); + for (int k = 0; k < j; ++k) { + int massIndex = freeLineage.at(k); + centerOfMass += _jointStates[massIndex].getPosition() - pivot; + } + // the gravitational effect is a rotation that tends to align the two cross products + const glm::vec3 worldAlignment = glm::vec3(0.0f, -1.f, 0.0f); + glm::quat gravityDelta = rotationBetween(glm::cross(centerOfMass, leverArm), + glm::cross(worldAlignment, leverArm)); + + float gravityAngle = glm::angle(gravityDelta); + const float MIN_GRAVITY_ANGLE = 0.1f; + float mixFactor = 0.5f; + if (gravityAngle < MIN_GRAVITY_ANGLE) { + // the final rotation is a mix of the two + mixFactor = 0.5f * gravityAngle / MIN_GRAVITY_ANGLE; + } + deltaRotation = safeMix(deltaRotation, gravityDelta, mixFactor); + } + */ + + // Apply the rotation, but use mixRotationDelta() which blends a bit of the default pose + // at in the process. This provides stability to the IK solution and removes the necessity + // for the gravity effect. + glm::quat oldNextRotation = nextState.getRotation(); + float mixFactor = 0.03f; + nextState.mixRotationDelta(deltaRotation, mixFactor, priority); + + // measure the result of the rotation which may have been modified by + // blending and constraints + glm::quat actualDelta = nextState.getRotation() * glm::inverse(oldNextRotation); + endPosition = pivot + actualDelta * leverArm; + } + + // recompute transforms from the top down + glm::mat4 parentTransform = topParentTransform; + for (int j = numFree - 1; j >= 0; --j) { + JointState& freeState = _jointStates[freeLineage.at(j)]; + freeState.computeTransform(parentTransform); + parentTransform = freeState.getTransform(); + } + + // measure our success + endPosition = endState.getPosition(); + distanceToGo = glm::distance(targetPosition, endPosition); + } while (numIterations < MAX_ITERATION_COUNT && distanceToGo < ACCEPTABLE_IK_ERROR); + + // set final rotation of the end joint + endState.setRotationFromBindFrame(targetRotation, priority); + + _shapesAreDirty = !_shapes.isEmpty(); +} + bool Model::restoreJointPosition(int jointIndex, float fraction, float priority) { if (jointIndex == -1 || _jointStates.isEmpty()) { return false; diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h index 8572831d8b..835207b7eb 100644 --- a/interface/src/renderer/Model.h +++ b/interface/src/renderer/Model.h @@ -194,6 +194,8 @@ protected: bool setJointPosition(int jointIndex, const glm::vec3& position, const glm::quat& rotation = glm::quat(), bool useRotation = false, int lastFreeIndex = -1, bool allIntermediatesFree = false, const glm::vec3& alignment = glm::vec3(0.0f, -1.0f, 0.0f), float priority = 1.0f); + + void inverseKinematics(int jointIndex, glm::vec3 position, const glm::quat& rotation, float priority); /// Restores the indexed joint to its default position. /// \param fraction the fraction of the default position to apply (i.e., 0.25f to slerp one fourth of the way to