move JointState to libraries

interface/src/avatar/SkeletonRagdoll.h

@@ -14,10 +14,9 @@
 
 #include <QVector>
 
+#include <JointState.h>
 #include <Ragdoll.h>
 
-#include "../renderer/JointState.h"
-
 class MuscleConstraint;
 class Model;
 

interface/src/renderer/Model.h

@@ -23,13 +23,13 @@
 #include <DependencyManager.h>
 #include <GeometryCache.h>
 #include <GeometryUtil.h>
+#include <JointState.h>
 #include <PhysicsEntity.h>
 #include <ProgramObject.h>
 #include <TextureCache.h>
 #include <Transform.h>
 
 #include "AnimationHandle.h"
-#include "JointState.h"
 
 class QScriptEngine;
 

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 <glm/gtx/norm.hpp>
+
+#include <QThreadPool>
+
+#include <AngularConstraint.h>
+#include <SharedUtil.h>
+
+#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<JointState*>(this)->_rotation = extractRotation(_transform);
+        const_cast<JointState*>(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;
+}

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 <glm/glm.hpp>
+#include <glm/gtx/quaternion.hpp>
+#include <glm/gtx/transform.hpp>
+
+#include <GLMHelpers.h>
+#include <FBXReader.h>
+
+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