diff --git a/interface/src/avatar/SkeletonRagdoll.h b/interface/src/avatar/SkeletonRagdoll.h index ae9bec9116..2f8ce1f712 100644 --- a/interface/src/avatar/SkeletonRagdoll.h +++ b/interface/src/avatar/SkeletonRagdoll.h @@ -14,10 +14,9 @@ #include +#include #include -#include "../renderer/JointState.h" - class MuscleConstraint; class Model; diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h index c79a6c2efd..8000e7385b 100644 --- a/interface/src/renderer/Model.h +++ b/interface/src/renderer/Model.h @@ -23,13 +23,13 @@ #include #include #include +#include #include #include #include #include #include "AnimationHandle.h" -#include "JointState.h" class QScriptEngine; diff --git a/libraries/render-utils/src/JointState.cpp b/libraries/render-utils/src/JointState.cpp new file mode 100644 index 0000000000..96561758da --- /dev/null +++ b/libraries/render-utils/src/JointState.cpp @@ -0,0 +1,281 @@ +// +// JointState.cpp +// interface/src/renderer +// +// Created by Andrzej Kapolka on 10/18/13. +// Copyright 2013 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#include + +#include + +#include +#include + +#include "JointState.h" + +JointState::JointState() : + _animationPriority(0.0f), + _transformChanged(true), + _rotationIsValid(false), + _positionInParentFrame(0.0f), + _distanceToParent(0.0f), + _fbxJoint(NULL), + _constraint(NULL) { +} + +JointState::JointState(const JointState& other) : _constraint(NULL) { + _transformChanged = other._transformChanged; + _transform = other._transform; + _rotationIsValid = other._rotationIsValid; + _rotation = other._rotation; + _rotationInConstrainedFrame = other._rotationInConstrainedFrame; + _positionInParentFrame = other._positionInParentFrame; + _distanceToParent = other._distanceToParent; + _animationPriority = other._animationPriority; + _fbxJoint = other._fbxJoint; + // DO NOT copy _constraint +} + +JointState::~JointState() { + delete _constraint; + _constraint = NULL; + if (_constraint) { + delete _constraint; + _constraint = NULL; + } +} + +glm::quat JointState::getRotation() const { + if (!_rotationIsValid) { + const_cast(this)->_rotation = extractRotation(_transform); + const_cast(this)->_rotationIsValid = true; + } + + return _rotation; +} + +void JointState::setFBXJoint(const FBXJoint* joint) { + assert(joint != NULL); + _rotationInConstrainedFrame = joint->rotation; + _transformChanged = true; + _rotationIsValid = false; + + // NOTE: JointState does not own the FBXJoint to which it points. + _fbxJoint = joint; + if (_constraint) { + delete _constraint; + _constraint = NULL; + } +} + +void JointState::buildConstraint() { + if (_constraint) { + delete _constraint; + _constraint = NULL; + } + if (glm::distance2(glm::vec3(-PI), _fbxJoint->rotationMin) > EPSILON || + glm::distance2(glm::vec3(PI), _fbxJoint->rotationMax) > EPSILON ) { + // this joint has rotation constraints + _constraint = AngularConstraint::newAngularConstraint(_fbxJoint->rotationMin, _fbxJoint->rotationMax); + } +} + +void JointState::copyState(const JointState& state) { + _animationPriority = state._animationPriority; + _transformChanged = state._transformChanged; + _transform = state._transform; + _rotationIsValid = state._rotationIsValid; + _rotation = state._rotation; + _rotationInConstrainedFrame = state._rotationInConstrainedFrame; + _positionInParentFrame = state._positionInParentFrame; + _distanceToParent = state._distanceToParent; + + _visibleTransform = state._visibleTransform; + _visibleRotation = extractRotation(_visibleTransform); + _visibleRotationInConstrainedFrame = state._visibleRotationInConstrainedFrame; + // DO NOT copy _fbxJoint or _constraint +} + +void JointState::initTransform(const glm::mat4& parentTransform) { + computeTransform(parentTransform); + _positionInParentFrame = glm::inverse(extractRotation(parentTransform)) * (extractTranslation(_transform) - extractTranslation(parentTransform)); + _distanceToParent = glm::length(_positionInParentFrame); +} + +void JointState::computeTransform(const glm::mat4& parentTransform, bool parentTransformChanged, bool synchronousRotationCompute) { + if (!parentTransformChanged && !_transformChanged) { + return; + } + + glm::quat rotationInParentFrame = _fbxJoint->preRotation * _rotationInConstrainedFrame * _fbxJoint->postRotation; + glm::mat4 transformInParentFrame = _fbxJoint->preTransform * glm::mat4_cast(rotationInParentFrame) * _fbxJoint->postTransform; + glm::mat4 newTransform = parentTransform * glm::translate(_fbxJoint->translation) * transformInParentFrame; + + if (newTransform != _transform) { + _transform = newTransform; + _transformChanged = true; + _rotationIsValid = false; + } +} + +void JointState::computeVisibleTransform(const glm::mat4& parentTransform) { + glm::quat rotationInParentFrame = _fbxJoint->preRotation * _visibleRotationInConstrainedFrame * _fbxJoint->postRotation; + glm::mat4 transformInParentFrame = _fbxJoint->preTransform * glm::mat4_cast(rotationInParentFrame) * _fbxJoint->postTransform; + _visibleTransform = parentTransform * glm::translate(_fbxJoint->translation) * transformInParentFrame; + _visibleRotation = extractRotation(_visibleTransform); +} + +glm::quat JointState::getRotationInBindFrame() const { + return getRotation() * _fbxJoint->inverseBindRotation; +} + +glm::quat JointState::getRotationInParentFrame() const { + return _fbxJoint->preRotation * _rotationInConstrainedFrame * _fbxJoint->postRotation; +} + +glm::quat JointState::getVisibleRotationInParentFrame() const { + return _fbxJoint->preRotation * _visibleRotationInConstrainedFrame * _fbxJoint->postRotation; +} + +void JointState::restoreRotation(float fraction, float priority) { + assert(_fbxJoint != NULL); + if (priority == _animationPriority || _animationPriority == 0.0f) { + setRotationInConstrainedFrameInternal(safeMix(_rotationInConstrainedFrame, _fbxJoint->rotation, fraction)); + _animationPriority = 0.0f; + } +} + +void JointState::setRotationInBindFrame(const glm::quat& rotation, float priority, bool constrain) { + // rotation is from bind- to model-frame + assert(_fbxJoint != NULL); + if (priority >= _animationPriority) { + glm::quat targetRotation = _rotationInConstrainedFrame * glm::inverse(getRotation()) * rotation * glm::inverse(_fbxJoint->inverseBindRotation); + if (constrain && _constraint) { + _constraint->softClamp(targetRotation, _rotationInConstrainedFrame, 0.5f); + } + setRotationInConstrainedFrameInternal(targetRotation); + _animationPriority = priority; + } +} + +void JointState::clearTransformTranslation() { + _transform[3][0] = 0.0f; + _transform[3][1] = 0.0f; + _transform[3][2] = 0.0f; + _transformChanged = true; + _visibleTransform[3][0] = 0.0f; + _visibleTransform[3][1] = 0.0f; + _visibleTransform[3][2] = 0.0f; +} + +void JointState::applyRotationDelta(const glm::quat& delta, bool constrain, float priority) { + // NOTE: delta is in model-frame + assert(_fbxJoint != NULL); + if (priority < _animationPriority || delta.null) { + return; + } + _animationPriority = priority; + glm::quat targetRotation = _rotationInConstrainedFrame * glm::inverse(getRotation()) * delta * getRotation(); + if (!constrain || _constraint == NULL) { + // no constraints + _rotationInConstrainedFrame = targetRotation; + _transformChanged = true; + + _rotation = delta * getRotation(); + return; + } + setRotationInConstrainedFrameInternal(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 = _rotationInConstrainedFrame * glm::inverse(getRotation()) * delta * getRotation(); + if (mixFactor > 0.0f && mixFactor <= 1.0f) { + targetRotation = safeMix(targetRotation, _fbxJoint->rotation, mixFactor); + } + if (_constraint) { + _constraint->softClamp(targetRotation, _rotationInConstrainedFrame, 0.5f); + } + setRotationInConstrainedFrameInternal(targetRotation); +} + +void JointState::mixVisibleRotationDelta(const glm::quat& delta, float mixFactor) { + // NOTE: delta is in model-frame + assert(_fbxJoint != NULL); + glm::quat targetRotation = _visibleRotationInConstrainedFrame * glm::inverse(_visibleRotation) * delta * _visibleRotation; + if (mixFactor > 0.0f && mixFactor <= 1.0f) { + //targetRotation = safeMix(targetRotation, _fbxJoint->rotation, mixFactor); + targetRotation = safeMix(targetRotation, _rotationInConstrainedFrame, mixFactor); + } + setVisibleRotationInConstrainedFrame(targetRotation); +} + +glm::quat JointState::computeParentRotation() const { + // R = Rp * Rpre * r * Rpost + // Rp = R * (Rpre * r * Rpost)^ + return getRotation() * glm::inverse(_fbxJoint->preRotation * _rotationInConstrainedFrame * _fbxJoint->postRotation); +} + +glm::quat JointState::computeVisibleParentRotation() const { + return _visibleRotation * glm::inverse(_fbxJoint->preRotation * _visibleRotationInConstrainedFrame * _fbxJoint->postRotation); +} + +void JointState::setRotationInConstrainedFrame(glm::quat targetRotation, float priority, bool constrain) { + if (priority >= _animationPriority || _animationPriority == 0.0f) { + if (constrain && _constraint) { + _constraint->softClamp(targetRotation, _rotationInConstrainedFrame, 0.5f); + } + setRotationInConstrainedFrameInternal(targetRotation); + _animationPriority = priority; + } +} + +void JointState::setRotationInConstrainedFrameInternal(const glm::quat& targetRotation) { + glm::quat parentRotation = computeParentRotation(); + _rotationInConstrainedFrame = targetRotation; + _transformChanged = true; + // R' = Rp * Rpre * r' * Rpost + _rotation = parentRotation * _fbxJoint->preRotation * _rotationInConstrainedFrame * _fbxJoint->postRotation; +} + +void JointState::setVisibleRotationInConstrainedFrame(const glm::quat& targetRotation) { + glm::quat parentRotation = computeVisibleParentRotation(); + _visibleRotationInConstrainedFrame = targetRotation; + _visibleRotation = parentRotation * _fbxJoint->preRotation * _visibleRotationInConstrainedFrame * _fbxJoint->postRotation; +} + +const bool JointState::rotationIsDefault(const glm::quat& rotation, float tolerance) const { + glm::quat defaultRotation = _fbxJoint->rotation; + return glm::abs(rotation.x - defaultRotation.x) < tolerance && + glm::abs(rotation.y - defaultRotation.y) < tolerance && + glm::abs(rotation.z - defaultRotation.z) < tolerance && + glm::abs(rotation.w - defaultRotation.w) < tolerance; +} + +glm::quat JointState::getDefaultRotationInParentFrame() const { + // NOTE: the result is constant and could be cached in the FBXJoint + return _fbxJoint->preRotation * _fbxJoint->rotation * _fbxJoint->postRotation; +} + +const glm::vec3& JointState::getDefaultTranslationInConstrainedFrame() const { + assert(_fbxJoint != NULL); + return _fbxJoint->translation; +} + +void JointState::slaveVisibleTransform() { + _visibleTransform = _transform; + _visibleRotation = getRotation(); + _visibleRotationInConstrainedFrame = _rotationInConstrainedFrame; +} diff --git a/libraries/render-utils/src/JointState.h b/libraries/render-utils/src/JointState.h new file mode 100644 index 0000000000..b502083463 --- /dev/null +++ b/libraries/render-utils/src/JointState.h @@ -0,0 +1,129 @@ +// +// JointState.h +// interface/src/renderer +// +// Created by Andrzej Kapolka on 10/18/13. +// Copyright 2013 High Fidelity, Inc. +// +// Distributed under the Apache License, Version 2.0. +// See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html +// + +#ifndef hifi_JointState_h +#define hifi_JointState_h + +#include +#include +#include + +#include +#include + +const float DEFAULT_PRIORITY = 3.0f; + +class AngularConstraint; + +class JointState { +public: + JointState(); + JointState(const JointState& other); + ~JointState(); + + void setFBXJoint(const FBXJoint* joint); + const FBXJoint& getFBXJoint() const { return *_fbxJoint; } + + void buildConstraint(); + void copyState(const JointState& state); + + void initTransform(const glm::mat4& parentTransform); + // if synchronousRotationCompute is true, then _transform is still computed synchronously, + // but _rotation will be asynchronously extracted + void computeTransform(const glm::mat4& parentTransform, bool parentTransformChanged = true, bool synchronousRotationCompute = false); + + 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; } + void resetTransformChanged() { _transformChanged = false; } + bool getTransformChanged() const { return _transformChanged; } + + glm::quat getRotation() const; + glm::vec3 getPosition() const { return extractTranslation(_transform); } + + /// \return rotation from bind to model frame + glm::quat getRotationInBindFrame() const; + + glm::quat getRotationInParentFrame() const; + glm::quat getVisibleRotationInParentFrame() const; + const glm::vec3& getPositionInParentFrame() const { return _positionInParentFrame; } + float getDistanceToParent() const { return _distanceToParent; } + + int getParentIndex() const { return _fbxJoint->parentIndex; } + + /// \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); + void mixVisibleRotationDelta(const glm::quat& delta, float mixFactor); + + /// 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 + /// computes and sets new _rotationInConstrainedFrame + /// NOTE: the JointState's model-frame transform/rotation are NOT updated! + void setRotationInBindFrame(const glm::quat& rotation, float priority, bool constrain = false); + + void setRotationInConstrainedFrame(glm::quat targetRotation, float priority, bool constrain = false); + void setVisibleRotationInConstrainedFrame(const glm::quat& targetRotation); + const glm::quat& getRotationInConstrainedFrame() const { return _rotationInConstrainedFrame; } + const glm::quat& getVisibleRotationInConstrainedFrame() const { return _visibleRotationInConstrainedFrame; } + + const bool rotationIsDefault(const glm::quat& rotation, float tolerance = EPSILON) const; + + glm::quat getDefaultRotationInParentFrame() const; + const glm::vec3& getDefaultTranslationInConstrainedFrame() const; + + + void clearTransformTranslation(); + + void slaveVisibleTransform(); + + float _animationPriority; // the priority of the animation affecting this joint + + /// \return parent model-frame rotation + // (used to keep _rotation consistent when modifying _rotationInWorldFrame directly) + glm::quat computeParentRotation() const; + glm::quat computeVisibleParentRotation() const; + +private: + void setRotationInConstrainedFrameInternal(const glm::quat& targetRotation); + /// debug helper function + void loadBindRotation(); + + bool _transformChanged; + glm::mat4 _transform; // joint- to model-frame + bool _rotationIsValid; + glm::quat _rotation; // joint- to model-frame + glm::quat _rotationInConstrainedFrame; // rotation in frame where angular constraints would be applied + glm::vec3 _positionInParentFrame; // only changes when the Model is scaled + float _distanceToParent; + + glm::mat4 _visibleTransform; + glm::quat _visibleRotation; + glm::quat _visibleRotationInConstrainedFrame; + + const FBXJoint* _fbxJoint; // JointState does NOT own its FBXJoint + AngularConstraint* _constraint; // JointState owns its AngularConstraint +}; + +#endif // hifi_JointState_h