From 29e093e2e079e8a95478d4333a212b88ccee9103 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Thu, 29 May 2014 09:20:01 -0700
Subject: [PATCH 1/9] fix float literals to agree with coding standard

---
 interface/src/renderer/Model.cpp | 24 ++++++++++++------------
 1 file changed, 12 insertions(+), 12 deletions(-)

diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index 1a857ff811..cc21ee1630 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -41,9 +41,9 @@ Model::Model(QObject* parent) :
     _snappedToCenter(false),
     _rootIndex(-1),
     _shapesAreDirty(true),
-    _boundingRadius(0.f),
+    _boundingRadius(0.0f),
     _boundingShape(), 
-    _boundingShapeLocalOffset(0.f),
+    _boundingShapeLocalOffset(0.0f),
     _lodDistance(0.0f),
     _pupilDilation(0.0f),
     _url("http://invalid.com") {
@@ -549,8 +549,8 @@ Extents Model::getMeshExtents() const {
 
     // even though our caller asked for "unscaled" we need to include any fst scaling, translation, and rotation, which
     // is captured in the offset matrix
-    glm::vec3 minimum = glm::vec3(_geometry->getFBXGeometry().offset * glm::vec4(extents.minimum, 1.0));
-    glm::vec3 maximum = glm::vec3(_geometry->getFBXGeometry().offset * glm::vec4(extents.maximum, 1.0));
+    glm::vec3 minimum = glm::vec3(_geometry->getFBXGeometry().offset * glm::vec4(extents.minimum, 1.0f));
+    glm::vec3 maximum = glm::vec3(_geometry->getFBXGeometry().offset * glm::vec4(extents.maximum, 1.0f));
     Extents scaledExtents = { minimum * _scale, maximum * _scale };
     return scaledExtents;
 }
@@ -564,8 +564,8 @@ Extents Model::getUnscaledMeshExtents() const {
 
     // even though our caller asked for "unscaled" we need to include any fst scaling, translation, and rotation, which
     // is captured in the offset matrix
-    glm::vec3 minimum = glm::vec3(_geometry->getFBXGeometry().offset * glm::vec4(extents.minimum, 1.0));
-    glm::vec3 maximum = glm::vec3(_geometry->getFBXGeometry().offset * glm::vec4(extents.maximum, 1.0));
+    glm::vec3 minimum = glm::vec3(_geometry->getFBXGeometry().offset * glm::vec4(extents.minimum, 1.0f));
+    glm::vec3 maximum = glm::vec3(_geometry->getFBXGeometry().offset * glm::vec4(extents.maximum, 1.0f));
     Extents scaledExtents = { minimum, maximum };
         
     return scaledExtents;
@@ -702,7 +702,7 @@ void Model::rebuildShapes() {
             // however we must have a shape for each joint, 
             // so we make a bogus sphere with zero radius.
             // TODO: implement collision groups for more control over what collides with what
-            SphereShape* sphere = new SphereShape(0.f, glm::vec3(0.0f)); 
+            SphereShape* sphere = new SphereShape(0.0f, glm::vec3(0.0f)); 
             _jointShapes.push_back(sphere);
         }
     }
@@ -844,8 +844,8 @@ void Model::resetShapePositions() {
 
 void Model::updateShapePositions() {
     if (_shapesAreDirty && _jointShapes.size() == _jointStates.size()) {
-        glm::vec3 rootPosition(0.f);
-        _boundingRadius = 0.f;
+        glm::vec3 rootPosition(0.0f);
+        _boundingRadius = 0.0f;
         float uniformScale = extractUniformScale(_scale);
         const FBXGeometry& geometry = _geometry->getFBXGeometry();
         for (int i = 0; i < _jointStates.size(); i++) {
@@ -1344,7 +1344,7 @@ void Model::renderJointCollisionShapes(float alpha) {
             glutSolidSphere(capsule->getRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS);
             
             // draw a green cylinder between the two points
-            glm::vec3 origin(0.f);
+            glm::vec3 origin(0.0f);
             glColor4f(0.6f, 0.8f, 0.6f, alpha);
             Avatar::renderJointConnectingCone( origin, axis, capsule->getRadius(), capsule->getRadius());
         }
@@ -1376,7 +1376,7 @@ void Model::renderBoundingCollisionShapes(float alpha) {
     glutSolidSphere(_boundingShape.getRadius(), BALL_SUBDIVISIONS, BALL_SUBDIVISIONS);
 
     // draw a green cylinder between the two points
-    glm::vec3 origin(0.f);
+    glm::vec3 origin(0.0f);
     glColor4f(0.6f, 0.8f, 0.6f, alpha);
     Avatar::renderJointConnectingCone( origin, axis, _boundingShape.getRadius(), _boundingShape.getRadius());
 
@@ -1404,7 +1404,7 @@ void Model::applyCollision(CollisionInfo& collision) {
         return;
     }
 
-    glm::vec3 jointPosition(0.f);
+    glm::vec3 jointPosition(0.0f);
     int jointIndex = collision._intData;
     if (getJointPosition(jointIndex, jointPosition)) {
         const FBXJoint& joint = _geometry->getFBXGeometry().joints[jointIndex];

From 4cf56287836f4a1638fa62b46548c24876577684 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Thu, 29 May 2014 10:00:55 -0700
Subject: [PATCH 2/9] move applyRotationDelta() from Model to JointState

---
 interface/src/avatar/SkeletonModel.cpp | 10 ++---
 interface/src/renderer/Model.cpp       | 59 +++++++++++++-------------
 interface/src/renderer/Model.h         |  7 ++-
 3 files changed, 37 insertions(+), 39 deletions(-)

diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index 3586e525ae..0b179e965c 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -131,7 +131,7 @@ bool operator<(const IndexValue& firstIndex, const IndexValue& secondIndex) {
 }
 
 void SkeletonModel::applyHandPosition(int jointIndex, const glm::vec3& position) {
-    if (jointIndex == -1) {
+    if (jointIndex == -1 || jointIndex >= _jointStates.size()) {
         return;
     }
     setJointPosition(jointIndex, position, glm::quat(), false, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY);
@@ -150,12 +150,12 @@ void SkeletonModel::applyHandPosition(int jointIndex, const glm::vec3& position)
 
     // align hand with forearm
     float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f;
-    applyRotationDelta(jointIndex, rotationBetween(handRotation * glm::vec3(-sign, 0.0f, 0.0f),
-        forearmVector), true, PALM_PRIORITY);
+    JointState& state = _jointStates[jointIndex];
+    state.applyRotationDelta(rotationBetween(handRotation * glm::vec3(-sign, 0.0f, 0.0f), forearmVector), true, PALM_PRIORITY);
 }
 
 void SkeletonModel::applyPalmData(int jointIndex, PalmData& palm) {
-    if (jointIndex == -1) {
+    if (jointIndex == -1 || jointIndex >= _jointStates.size()) {
         return;
     }
     const FBXGeometry& geometry = _geometry->getFBXGeometry();
@@ -245,7 +245,7 @@ void SkeletonModel::maybeUpdateEyeRotation(const JointState& parentState, const
 }
 
 void SkeletonModel::renderJointConstraints(int jointIndex) {
-    if (jointIndex == -1) {
+    if (jointIndex == -1 || jointIndex >= _jointStates.size()) {
         return;
     }
     const FBXGeometry& geometry = _geometry->getFBXGeometry();
diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index cc21ee1630..6f9d7f2d17 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -1155,8 +1155,9 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& translation, const
         // first, try to rotate the end effector as close as possible to the target rotation, if any
         glm::quat endRotation;
         if (useRotation) {
+            JointState& state = _jointStates[jointIndex];
             getJointRotation(jointIndex, endRotation, true);
-            applyRotationDelta(jointIndex, rotation * glm::inverse(endRotation), true, priority);
+            state.applyRotationDelta(rotation * glm::inverse(endRotation), true, priority);
             getJointRotation(jointIndex, endRotation, true);
         }    
         
@@ -1200,7 +1201,7 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& translation, const
                         1.0f / (combinedWeight + 1.0f));
                 }
             }
-            applyRotationDelta(index, combinedDelta, true, priority);
+            state.applyRotationDelta(combinedDelta, true, priority);
             glm::quat actualDelta = state._combinedRotation * glm::inverse(oldCombinedRotation);
             endPosition = actualDelta * jointVector + jointPosition;
             if (useRotation) {
@@ -1281,27 +1282,6 @@ float Model::getLimbLength(int jointIndex) const {
     return length;
 }
 
-void Model::applyRotationDelta(int jointIndex, const glm::quat& delta, bool constrain, float priority) {
-    JointState& state = _jointStates[jointIndex];
-    if (priority < state._animationPriority) {
-        return;
-    }
-    state._animationPriority = priority;
-    const FBXJoint& joint = state.getFBXJoint();
-    if (!constrain || (joint.rotationMin == glm::vec3(-PI, -PI, -PI) &&
-            joint.rotationMax == glm::vec3(PI, PI, PI))) {
-        // no constraints
-        state._rotation = state._rotation * glm::inverse(state._combinedRotation) * delta * state._combinedRotation;
-        state._combinedRotation = delta * state._combinedRotation;
-        return;
-    }
-    glm::quat targetRotation = delta * state._combinedRotation;
-    glm::vec3 eulers = safeEulerAngles(state._rotation * glm::inverse(state._combinedRotation) * targetRotation);
-    glm::quat newRotation = glm::quat(glm::clamp(eulers, joint.rotationMin, joint.rotationMax));
-    state._combinedRotation = state._combinedRotation * glm::inverse(state._rotation) * newRotation;
-    state._rotation = newRotation;
-}
-
 const int BALL_SUBDIVISIONS = 10;
 
 void Model::renderJointCollisionShapes(float alpha) {
@@ -1899,13 +1879,6 @@ void JointState::setFBXJoint(const FBXJoint* joint) {
     _fbxJoint = joint;
 }
 
-void JointState::updateWorldTransform(const glm::mat4& baseTransform, const glm::quat& parentRotation) {
-    assert(_fbxJoint != NULL);
-    glm::quat combinedRotation = _fbxJoint->preRotation * _rotation * _fbxJoint->postRotation;    
-    _transform = baseTransform * glm::translate(_translation) * _fbxJoint->preTransform * glm::mat4_cast(combinedRotation) * _fbxJoint->postTransform;
-    _combinedRotation = parentRotation * combinedRotation;
-}
-
 void JointState::copyState(const JointState& state) {
     _translation = state._translation;
     _rotation = state._rotation;
@@ -1914,3 +1887,29 @@ void JointState::copyState(const JointState& state) {
     _animationPriority = state._animationPriority;
     // DO NOT copy _fbxJoint
 }
+
+void JointState::updateWorldTransform(const glm::mat4& baseTransform, const glm::quat& parentRotation) {
+    assert(_fbxJoint != NULL);
+    glm::quat combinedRotation = _fbxJoint->preRotation * _rotation * _fbxJoint->postRotation;    
+    _transform = baseTransform * glm::translate(_translation) * _fbxJoint->preTransform * glm::mat4_cast(combinedRotation) * _fbxJoint->postTransform;
+    _combinedRotation = parentRotation * combinedRotation;
+}
+
+void JointState::applyRotationDelta(const glm::quat& delta, bool constrain, float priority) {
+    if (priority < _animationPriority) {
+        return;
+    }
+    _animationPriority = priority;
+    if (!constrain || (_fbxJoint->rotationMin == glm::vec3(-PI, -PI, -PI) &&
+            _fbxJoint->rotationMax == glm::vec3(PI, PI, PI))) {
+        // no constraints
+        _rotation = _rotation * glm::inverse(_combinedRotation) * delta * _combinedRotation;
+        _combinedRotation = delta * _combinedRotation;
+        return;
+    }
+    glm::quat targetRotation = delta * _combinedRotation;
+    glm::vec3 eulers = safeEulerAngles(_rotation * glm::inverse(_combinedRotation) * targetRotation);
+    glm::quat newRotation = glm::quat(glm::clamp(eulers, _fbxJoint->rotationMin, _fbxJoint->rotationMax));
+    _combinedRotation = _combinedRotation * glm::inverse(_rotation) * newRotation;
+    _rotation = newRotation;
+}
diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h
index b8a5de5902..a6368e3388 100644
--- a/interface/src/renderer/Model.h
+++ b/interface/src/renderer/Model.h
@@ -38,10 +38,11 @@ public:
     void setFBXJoint(const FBXJoint* joint); 
     const FBXJoint& getFBXJoint() const { return *_fbxJoint; }
 
-    void updateWorldTransform(const glm::mat4& baseTransform, const glm::quat& parentRotation);
-
     void copyState(const JointState& state);
 
+    void updateWorldTransform(const glm::mat4& baseTransform, const glm::quat& parentRotation);
+    void applyRotationDelta(const glm::quat& delta, bool constrain = true, float priority = 1.0f);
+
     glm::vec3 _translation;  // translation relative to parent
     glm::quat _rotation;     // rotation relative to parent
     glm::mat4 _transform;    // rotation to world frame + translation in model frame
@@ -247,8 +248,6 @@ protected:
     /// first free ancestor.
     float getLimbLength(int jointIndex) const;
 
-    void applyRotationDelta(int jointIndex, const glm::quat& delta, bool constrain = true, float priority = 1.0f);
-    
     void computeBoundingShape(const FBXGeometry& geometry);
 
 private:

From a898fcbf75b5990259d84b78bc1fdbb6006acac4 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Thu, 29 May 2014 11:43:25 -0700
Subject: [PATCH 3/9] move details of getJointRotation() into JointState

also: remove unused getLeft/RightHandRotation() methods
---
 interface/src/avatar/SkeletonModel.cpp | 19 ++++++-------------
 interface/src/avatar/SkeletonModel.h   |  8 --------
 interface/src/renderer/Model.cpp       | 16 ++++++++++------
 interface/src/renderer/Model.h         |  4 ++++
 4 files changed, 20 insertions(+), 27 deletions(-)

diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index 0b179e965c..0223f390f8 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -145,12 +145,11 @@ void SkeletonModel::applyHandPosition(int jointIndex, const glm::vec3& position)
     if (forearmLength < EPSILON) {
         return;
     }
-    glm::quat handRotation;
-    getJointRotation(jointIndex, handRotation, true);
+    JointState& state = _jointStates[jointIndex];
+    glm::quat handRotation = state.getJointRotation(true);
 
     // align hand with forearm
     float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f;
-    JointState& state = _jointStates[jointIndex];
     state.applyRotationDelta(rotationBetween(handRotation * glm::vec3(-sign, 0.0f, 0.0f), forearmVector), true, PALM_PRIORITY);
 }
 
@@ -169,9 +168,11 @@ void SkeletonModel::applyPalmData(int jointIndex, PalmData& palm) {
     glm::quat palmRotation;
     if (!Menu::getInstance()->isOptionChecked(MenuOption::AlternateIK) &&
             Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) {
-        getJointRotation(parentJointIndex, palmRotation, true);
+        JointState parentState = _jointStates[parentJointIndex];
+        palmRotation = parentState.getJointRotation(true);
     } else {
-        getJointRotation(jointIndex, palmRotation, true);
+        JointState state = _jointStates[jointIndex];
+        palmRotation = state.getJointRotation(true);
     }
     palmRotation = rotationBetween(palmRotation * geometry.palmDirection, palm.getNormal()) * palmRotation;
     
@@ -367,18 +368,10 @@ bool SkeletonModel::getLeftHandPosition(glm::vec3& position) const {
     return getJointPosition(getLeftHandJointIndex(), position);
 }
 
-bool SkeletonModel::getLeftHandRotation(glm::quat& rotation) const {
-    return getJointRotation(getLeftHandJointIndex(), rotation);
-}
-
 bool SkeletonModel::getRightHandPosition(glm::vec3& position) const {
     return getJointPosition(getRightHandJointIndex(), position);
 }
 
-bool SkeletonModel::getRightHandRotation(glm::quat& rotation) const {
-    return getJointRotation(getRightHandJointIndex(), rotation);
-}
-
 bool SkeletonModel::restoreLeftHandPosition(float percent, float priority) {
     return restoreJointPosition(getLeftHandJointIndex(), percent, priority);
 }
diff --git a/interface/src/avatar/SkeletonModel.h b/interface/src/avatar/SkeletonModel.h
index 60e925b239..8c5cfed23a 100644
--- a/interface/src/avatar/SkeletonModel.h
+++ b/interface/src/avatar/SkeletonModel.h
@@ -45,18 +45,10 @@ public:
     /// \return true whether or not the position was found
     bool getLeftHandPosition(glm::vec3& position) const;
     
-    /// Retrieve the rotation of the left hand
-    /// \return true whether or not the rotation was found
-    bool getLeftHandRotation(glm::quat& rotation) const;
-    
     /// Retrieve the position of the right hand
     /// \return true whether or not the position was found
     bool getRightHandPosition(glm::vec3& position) const;
     
-    /// Retrieve the rotation of the right hand
-    /// \return true whether or not the rotation was found
-    bool getRightHandRotation(glm::quat& rotation) const;
-    
     /// Restores some percentage of the default position of the left hand.
     /// \param percent the percentage of the default position to restore
     /// \return whether or not the left hand joint was found
diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index 6f9d7f2d17..b77b4324db 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -630,12 +630,10 @@ bool Model::getJointPosition(int jointIndex, glm::vec3& position) const {
 }
 
 bool Model::getJointRotation(int jointIndex, glm::quat& rotation, bool fromBind) const {
-    if (jointIndex == -1 || _jointStates.isEmpty()) {
+    if (jointIndex == -1 || jointIndex >= _jointStates.size()) {
         return false;
     }
-    rotation = _jointStates[jointIndex]._combinedRotation *
-        (fromBind ? _geometry->getFBXGeometry().joints[jointIndex].inverseBindRotation :
-            _geometry->getFBXGeometry().joints[jointIndex].inverseDefaultRotation);
+    rotation = _jointStates[jointIndex].getJointRotation(fromBind);
     return true;
 }
 
@@ -1156,9 +1154,11 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& translation, const
         glm::quat endRotation;
         if (useRotation) {
             JointState& state = _jointStates[jointIndex];
-            getJointRotation(jointIndex, endRotation, true);
+
+            // TODO: figure out what this is trying to do and combine it into one JointState method
+            endRotation = state.getJointRotation(true);
             state.applyRotationDelta(rotation * glm::inverse(endRotation), true, priority);
-            getJointRotation(jointIndex, endRotation, true);
+            endRotation = state.getJointRotation(true);
         }    
         
         // then, we go from the joint upwards, rotating the end as close as possible to the target
@@ -1895,6 +1895,10 @@ void JointState::updateWorldTransform(const glm::mat4& baseTransform, const glm:
     _combinedRotation = parentRotation * combinedRotation;
 }
 
+glm::quat JointState::getJointRotation(bool fromBind) const {
+    return _combinedRotation * (fromBind ?  _fbxJoint->inverseBindRotation : _fbxJoint->inverseDefaultRotation);
+}
+
 void JointState::applyRotationDelta(const glm::quat& delta, bool constrain, float priority) {
     if (priority < _animationPriority) {
         return;
diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h
index a6368e3388..f7ec181fb4 100644
--- a/interface/src/renderer/Model.h
+++ b/interface/src/renderer/Model.h
@@ -41,6 +41,10 @@ public:
     void copyState(const JointState& state);
 
     void updateWorldTransform(const glm::mat4& baseTransform, const glm::quat& parentRotation);
+
+    /// \return rotation from the joint's default (or bind) orientation
+    glm::quat getJointRotation(bool fromBind = false) const;
+
     void applyRotationDelta(const glm::quat& delta, bool constrain = true, float priority = 1.0f);
 
     glm::vec3 _translation;  // translation relative to parent

From 2e99d316acf4dd899ca2bcdf1ea6211e28b90873 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Thu, 29 May 2014 12:52:05 -0700
Subject: [PATCH 4/9] remove JointState::_translation

(use corresponding data in FBXJoint instead)
---
 interface/src/avatar/FaceModel.cpp     |  4 ++--
 interface/src/avatar/SkeletonModel.cpp |  2 +-
 interface/src/renderer/Model.cpp       | 27 +++++---------------------
 interface/src/renderer/Model.h         |  7 +++----
 4 files changed, 11 insertions(+), 29 deletions(-)

diff --git a/interface/src/avatar/FaceModel.cpp b/interface/src/avatar/FaceModel.cpp
index 6ecc45e1e3..acd3d2c31f 100644
--- a/interface/src/avatar/FaceModel.cpp
+++ b/interface/src/avatar/FaceModel.cpp
@@ -49,7 +49,7 @@ void FaceModel::simulate(float deltaTime, bool fullUpdate) {
 void FaceModel::maybeUpdateNeckRotation(const JointState& parentState, const FBXJoint& joint, JointState& state) {
     // get the rotation axes in joint space and use them to adjust the rotation
     glm::mat3 axes = glm::mat3_cast(_rotation);
-    glm::mat3 inverse = glm::mat3(glm::inverse(parentState._transform * glm::translate(state._translation) *
+    glm::mat3 inverse = glm::mat3(glm::inverse(parentState._transform * glm::translate(state.getDefaultTranslationInParentFrame()) *
         joint.preTransform * glm::mat4_cast(joint.preRotation)));
     state._rotation = glm::angleAxis(- RADIANS_PER_DEGREE * _owningHead->getFinalRoll(), glm::normalize(inverse * axes[2])) 
         * glm::angleAxis(RADIANS_PER_DEGREE * _owningHead->getFinalYaw(), glm::normalize(inverse * axes[1])) 
@@ -59,7 +59,7 @@ void FaceModel::maybeUpdateNeckRotation(const JointState& parentState, const FBX
 
 void FaceModel::maybeUpdateEyeRotation(const JointState& parentState, const FBXJoint& joint, JointState& state) {
     // likewise with the eye joints
-    glm::mat4 inverse = glm::inverse(parentState._transform * glm::translate(state._translation) *
+    glm::mat4 inverse = glm::inverse(parentState._transform * glm::translate(state.getDefaultTranslationInParentFrame()) *
         joint.preTransform * glm::mat4_cast(joint.preRotation * joint.rotation));
     glm::vec3 front = glm::vec3(inverse * glm::vec4(_owningHead->getFinalOrientation() * IDENTITY_FRONT, 0.0f));
     glm::vec3 lookAt = glm::vec3(inverse * glm::vec4(_owningHead->getLookAtPosition() +
diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index 0223f390f8..d40c660c4b 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -230,7 +230,7 @@ void SkeletonModel::maybeUpdateLeanRotation(const JointState& parentState, const
     }
     // get the rotation axes in joint space and use them to adjust the rotation
     glm::mat3 axes = glm::mat3_cast(_rotation);
-    glm::mat3 inverse = glm::mat3(glm::inverse(parentState._transform * glm::translate(state._translation) * 
+    glm::mat3 inverse = glm::mat3(glm::inverse(parentState._transform * glm::translate(state.getDefaultTranslationInParentFrame()) *
         joint.preTransform * glm::mat4_cast(joint.preRotation * joint.rotation)));
     state._rotation = glm::angleAxis(- RADIANS_PER_DEGREE * _owningAvatar->getHead()->getFinalLeanSideways(), 
         glm::normalize(inverse * axes[2])) * glm::angleAxis(- RADIANS_PER_DEGREE * _owningAvatar->getHead()->getFinalLeanForward(), 
diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index b77b4324db..065d4bf0f0 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -1232,23 +1232,6 @@ bool Model::setJointRotation(int jointIndex, const glm::quat& rotation, float pr
     return true;
 }
 
-void Model::setJointTranslation(int jointIndex, const glm::vec3& translation) {
-    JointState& state = _jointStates[jointIndex];
-    const FBXJoint& joint = state.getFBXJoint();
-    
-    glm::mat4 parentTransform;
-    if (joint.parentIndex == -1) {
-        const FBXGeometry& geometry = _geometry->getFBXGeometry();
-        parentTransform = glm::mat4_cast(_rotation) * glm::scale(_scale) * glm::translate(_offset) * geometry.offset;
-        
-    } else {
-        parentTransform = _jointStates.at(joint.parentIndex)._transform;
-    }
-    glm::vec3 preTranslation = extractTranslation(joint.preTransform * glm::mat4_cast(joint.preRotation *
-        state._rotation * joint.postRotation) * joint.postTransform); 
-    state._translation = glm::vec3(glm::inverse(parentTransform) * glm::vec4(translation, 1.0f)) - preTranslation;
-}
-
 bool Model::restoreJointPosition(int jointIndex, float percent, float priority) {
     if (jointIndex == -1 || _jointStates.isEmpty()) {
         return false;
@@ -1261,7 +1244,6 @@ bool Model::restoreJointPosition(int jointIndex, float percent, float priority)
         if (priority == state._animationPriority) {
             const FBXJoint& joint = geometry.joints.at(index);
             state._rotation = safeMix(state._rotation, joint.rotation, percent);
-            state._translation = glm::mix(state._translation, joint.translation, percent);
             state._animationPriority = 0.0f;
         }
     }
@@ -1866,21 +1848,18 @@ void AnimationHandle::replaceMatchingPriorities(float newPriority) {
 // JointState  TODO: move this class to its own files
 // ----------------------------------------------------------------------------
 JointState::JointState() :
-    _translation(0.0f),
     _animationPriority(0.0f),
     _fbxJoint(NULL) {
 }
 
 void JointState::setFBXJoint(const FBXJoint* joint) { 
     assert(joint != NULL);
-    _translation = joint->translation;
     _rotation = joint->rotation;
     // NOTE: JointState does not own the FBXJoint to which it points.
     _fbxJoint = joint;
 }
 
 void JointState::copyState(const JointState& state) {
-    _translation = state._translation;
     _rotation = state._rotation;
     _transform = state._transform;
     _combinedRotation = state._combinedRotation;
@@ -1891,7 +1870,7 @@ void JointState::copyState(const JointState& state) {
 void JointState::updateWorldTransform(const glm::mat4& baseTransform, const glm::quat& parentRotation) {
     assert(_fbxJoint != NULL);
     glm::quat combinedRotation = _fbxJoint->preRotation * _rotation * _fbxJoint->postRotation;    
-    _transform = baseTransform * glm::translate(_translation) * _fbxJoint->preTransform * glm::mat4_cast(combinedRotation) * _fbxJoint->postTransform;
+    _transform = baseTransform * glm::translate(_fbxJoint->translation) * _fbxJoint->preTransform * glm::mat4_cast(combinedRotation) * _fbxJoint->postTransform;
     _combinedRotation = parentRotation * combinedRotation;
 }
 
@@ -1917,3 +1896,7 @@ void JointState::applyRotationDelta(const glm::quat& delta, bool constrain, floa
     _combinedRotation = _combinedRotation * glm::inverse(_rotation) * newRotation;
     _rotation = newRotation;
 }
+
+const glm::vec3& JointState::getDefaultTranslationInParentFrame() const {
+    return _fbxJoint->translation;
+}
diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h
index f7ec181fb4..f346e5e10c 100644
--- a/interface/src/renderer/Model.h
+++ b/interface/src/renderer/Model.h
@@ -42,12 +42,13 @@ public:
 
     void updateWorldTransform(const glm::mat4& baseTransform, const glm::quat& parentRotation);
 
-    /// \return rotation from the joint's default (or bind) orientation
+    /// \return rotation from the joint's default (or bind) frame to world frame
     glm::quat getJointRotation(bool fromBind = false) const;
 
     void applyRotationDelta(const glm::quat& delta, bool constrain = true, float priority = 1.0f);
 
-    glm::vec3 _translation;  // translation relative to parent
+    const glm::vec3& getDefaultTranslationInParentFrame() const;
+
     glm::quat _rotation;     // rotation relative to parent
     glm::mat4 _transform;    // rotation to world frame + translation in model frame
     glm::quat _combinedRotation; // rotation from joint local to world frame
@@ -240,8 +241,6 @@ protected:
         const glm::vec3& alignment = glm::vec3(0.0f, -1.0f, 0.0f), float priority = 1.0f);
     bool setJointRotation(int jointIndex, const glm::quat& rotation, float priority = 1.0f);
     
-    void setJointTranslation(int jointIndex, const glm::vec3& translation);
-    
     /// Restores the indexed joint to its default position.
     /// \param percent the percentage of the default position to apply (i.e., 0.25f to slerp one fourth of the way to
     /// the original position

From 603333f6342bcdcb9cffd3dfc3cf97a50ab05feb Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Thu, 29 May 2014 14:11:17 -0700
Subject: [PATCH 5/9] add JointState::restoreRotation()

---
 interface/src/renderer/Model.cpp | 13 ++++++++-----
 interface/src/renderer/Model.h   |  2 ++
 2 files changed, 10 insertions(+), 5 deletions(-)

diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index 065d4bf0f0..d4d344a862 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -1241,11 +1241,7 @@ bool Model::restoreJointPosition(int jointIndex, float percent, float priority)
     
     foreach (int index, freeLineage) {
         JointState& state = _jointStates[index];
-        if (priority == state._animationPriority) {
-            const FBXJoint& joint = geometry.joints.at(index);
-            state._rotation = safeMix(state._rotation, joint.rotation, percent);
-            state._animationPriority = 0.0f;
-        }
+        state.restoreRotation(percent, priority);
     }
     return true;
 }
@@ -1878,6 +1874,13 @@ glm::quat JointState::getJointRotation(bool fromBind) const {
     return _combinedRotation * (fromBind ?  _fbxJoint->inverseBindRotation : _fbxJoint->inverseDefaultRotation);
 }
 
+void JointState::restoreRotation(float percent, float priority) {
+    if (priority == _animationPriority) {
+        _rotation = safeMix(_rotation, _fbxJoint->rotation, percent);
+        _animationPriority = 0.0f;
+    }
+}
+
 void JointState::applyRotationDelta(const glm::quat& delta, bool constrain, float priority) {
     if (priority < _animationPriority) {
         return;
diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h
index f346e5e10c..c2b7e80026 100644
--- a/interface/src/renderer/Model.h
+++ b/interface/src/renderer/Model.h
@@ -49,6 +49,8 @@ public:
 
     const glm::vec3& getDefaultTranslationInParentFrame() const;
 
+    void restoreRotation(float percent, float priority);
+
     glm::quat _rotation;     // rotation relative to parent
     glm::mat4 _transform;    // rotation to world frame + translation in model frame
     glm::quat _combinedRotation; // rotation from joint local to world frame

From 6efa85110405c06701d1b3a724587f9bee7655be Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Thu, 29 May 2014 15:44:27 -0700
Subject: [PATCH 6/9] minor cleanup + use 'fraction' instead of 'percent'

---
 interface/src/avatar/SkeletonModel.cpp |  8 ++++----
 interface/src/avatar/SkeletonModel.h   | 12 ++++++------
 interface/src/renderer/Model.cpp       | 13 ++++++-------
 interface/src/renderer/Model.h         |  6 +++---
 4 files changed, 19 insertions(+), 20 deletions(-)

diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index d40c660c4b..711e11def6 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -372,8 +372,8 @@ bool SkeletonModel::getRightHandPosition(glm::vec3& position) const {
     return getJointPosition(getRightHandJointIndex(), position);
 }
 
-bool SkeletonModel::restoreLeftHandPosition(float percent, float priority) {
-    return restoreJointPosition(getLeftHandJointIndex(), percent, priority);
+bool SkeletonModel::restoreLeftHandPosition(float fraction, float priority) {
+    return restoreJointPosition(getLeftHandJointIndex(), fraction, priority);
 }
 
 bool SkeletonModel::getLeftShoulderPosition(glm::vec3& position) const {
@@ -384,8 +384,8 @@ float SkeletonModel::getLeftArmLength() const {
     return getLimbLength(getLeftHandJointIndex());
 }
 
-bool SkeletonModel::restoreRightHandPosition(float percent, float priority) {
-    return restoreJointPosition(getRightHandJointIndex(), percent, priority);
+bool SkeletonModel::restoreRightHandPosition(float fraction, float priority) {
+    return restoreJointPosition(getRightHandJointIndex(), fraction, priority);
 }
 
 bool SkeletonModel::getRightShoulderPosition(glm::vec3& position) const {
diff --git a/interface/src/avatar/SkeletonModel.h b/interface/src/avatar/SkeletonModel.h
index 8c5cfed23a..91070e4ad6 100644
--- a/interface/src/avatar/SkeletonModel.h
+++ b/interface/src/avatar/SkeletonModel.h
@@ -49,10 +49,10 @@ public:
     /// \return true whether or not the position was found
     bool getRightHandPosition(glm::vec3& position) const;
     
-    /// Restores some percentage of the default position of the left hand.
-    /// \param percent the percentage of the default position to restore
+    /// Restores some fraction of the default position of the left hand.
+    /// \param fraction the fraction of the default position to restore
     /// \return whether or not the left hand joint was found
-    bool restoreLeftHandPosition(float percent = 1.0f, float priority = 1.0f);
+    bool restoreLeftHandPosition(float fraction = 1.0f, float priority = 1.0f);
     
     /// Gets the position of the left shoulder.
     /// \return whether or not the left shoulder joint was found
@@ -61,10 +61,10 @@ public:
     /// Returns the extended length from the left hand to its last free ancestor.
     float getLeftArmLength() const;
     
-    /// Restores some percentage of the default position of the right hand.
-    /// \param percent the percentage of the default position to restore
+    /// Restores some fraction of the default position of the right hand.
+    /// \param fraction the fraction of the default position to restore
     /// \return whether or not the right hand joint was found
-    bool restoreRightHandPosition(float percent = 1.0f, float priority = 1.0f);
+    bool restoreRightHandPosition(float fraction = 1.0f, float priority = 1.0f);
     
     /// Gets the position of the right shoulder.
     /// \return whether or not the right shoulder joint was found
diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index d4d344a862..bae1e7246d 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -590,9 +590,8 @@ void Model::setJointState(int index, bool valid, const glm::quat& rotation, floa
             if (valid) {
                 state._rotation = rotation;
                 state._animationPriority = priority;
-            } else if (priority == state._animationPriority) {
-                state._rotation = _geometry->getFBXGeometry().joints.at(index).rotation;
-                state._animationPriority = 0.0f;
+            } else {
+                state.restoreRotation(1.0f, priority);
             }
         }
     }
@@ -1232,7 +1231,7 @@ bool Model::setJointRotation(int jointIndex, const glm::quat& rotation, float pr
     return true;
 }
 
-bool Model::restoreJointPosition(int jointIndex, float percent, float priority) {
+bool Model::restoreJointPosition(int jointIndex, float fraction, float priority) {
     if (jointIndex == -1 || _jointStates.isEmpty()) {
         return false;
     }
@@ -1241,7 +1240,7 @@ bool Model::restoreJointPosition(int jointIndex, float percent, float priority)
     
     foreach (int index, freeLineage) {
         JointState& state = _jointStates[index];
-        state.restoreRotation(percent, priority);
+        state.restoreRotation(fraction, priority);
     }
     return true;
 }
@@ -1874,9 +1873,9 @@ glm::quat JointState::getJointRotation(bool fromBind) const {
     return _combinedRotation * (fromBind ?  _fbxJoint->inverseBindRotation : _fbxJoint->inverseDefaultRotation);
 }
 
-void JointState::restoreRotation(float percent, float priority) {
+void JointState::restoreRotation(float fraction, float priority) {
     if (priority == _animationPriority) {
-        _rotation = safeMix(_rotation, _fbxJoint->rotation, percent);
+        _rotation = safeMix(_rotation, _fbxJoint->rotation, fraction);
         _animationPriority = 0.0f;
     }
 }
diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h
index c2b7e80026..de6a760e0f 100644
--- a/interface/src/renderer/Model.h
+++ b/interface/src/renderer/Model.h
@@ -49,7 +49,7 @@ public:
 
     const glm::vec3& getDefaultTranslationInParentFrame() const;
 
-    void restoreRotation(float percent, float priority);
+    void restoreRotation(float fraction, float priority);
 
     glm::quat _rotation;     // rotation relative to parent
     glm::mat4 _transform;    // rotation to world frame + translation in model frame
@@ -244,10 +244,10 @@ protected:
     bool setJointRotation(int jointIndex, const glm::quat& rotation, float priority = 1.0f);
     
     /// Restores the indexed joint to its default position.
-    /// \param percent the percentage of the default position to apply (i.e., 0.25f to slerp one fourth of the way to
+    /// \param fraction the fraction of the default position to apply (i.e., 0.25f to slerp one fourth of the way to
     /// the original position
     /// \return true if the joint was found
-    bool restoreJointPosition(int jointIndex, float percent = 1.0f, float priority = 0.0f);
+    bool restoreJointPosition(int jointIndex, float fraction = 1.0f, float priority = 0.0f);
     
     /// Computes and returns the extended length of the limb terminating at the specified joint and starting at the joint's
     /// first free ancestor.

From e21dbd87263fd52d32c0993377b2afaf6becb1d1 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Fri, 30 May 2014 08:20:11 -0700
Subject: [PATCH 7/9] rename updateWorldTransforms to computeTransforms

---
 interface/src/renderer/Model.cpp | 10 +++++-----
 interface/src/renderer/Model.h   |  2 +-
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index bae1e7246d..b716c48603 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -164,12 +164,12 @@ QVector<JointState> Model::createJointStates(const FBXGeometry& geometry) {
             if (parentIndex == -1) {
                 _rootIndex = i;
                 glm::mat4 baseTransform = glm::mat4_cast(_rotation) * glm::scale(_scale) * glm::translate(_offset) * geometry.offset;
-                state.updateWorldTransform(baseTransform, _rotation);
+                state.computeTransforms(baseTransform, _rotation);
                 ++numJointsSet;
                 jointIsSet[i] = true;
             } else if (jointIsSet[parentIndex]) {
                 const JointState& parentState = jointStates.at(parentIndex);
-                state.updateWorldTransform(parentState._transform, parentState._combinedRotation);
+                state.computeTransforms(parentState._transform, parentState._combinedRotation);
                 ++numJointsSet;
                 jointIsSet[i] = true;
             }
@@ -1122,10 +1122,10 @@ void Model::updateJointState(int index) {
     if (joint.parentIndex == -1) {
         const FBXGeometry& geometry = _geometry->getFBXGeometry();
         glm::mat4 baseTransform = glm::mat4_cast(_rotation) * glm::scale(_scale) * glm::translate(_offset) * geometry.offset;
-        state.updateWorldTransform(baseTransform, _rotation);
+        state.computeTransforms(baseTransform, _rotation);
     } else {
         const JointState& parentState = _jointStates.at(joint.parentIndex);
-        state.updateWorldTransform(parentState._transform, parentState._combinedRotation);
+        state.computeTransforms(parentState._transform, parentState._combinedRotation);
     }
 }
 
@@ -1862,7 +1862,7 @@ void JointState::copyState(const JointState& state) {
     // DO NOT copy _fbxJoint
 }
 
-void JointState::updateWorldTransform(const glm::mat4& baseTransform, const glm::quat& parentRotation) {
+void JointState::computeTransforms(const glm::mat4& baseTransform, const glm::quat& parentRotation) {
     assert(_fbxJoint != NULL);
     glm::quat combinedRotation = _fbxJoint->preRotation * _rotation * _fbxJoint->postRotation;    
     _transform = baseTransform * glm::translate(_fbxJoint->translation) * _fbxJoint->preTransform * glm::mat4_cast(combinedRotation) * _fbxJoint->postTransform;
diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h
index de6a760e0f..deec5aeee9 100644
--- a/interface/src/renderer/Model.h
+++ b/interface/src/renderer/Model.h
@@ -40,7 +40,7 @@ public:
 
     void copyState(const JointState& state);
 
-    void updateWorldTransform(const glm::mat4& baseTransform, const glm::quat& parentRotation);
+    void computeTransforms(const glm::mat4& baseTransform, const glm::quat& parentRotation);
 
     /// \return rotation from the joint's default (or bind) frame to world frame
     glm::quat getJointRotation(bool fromBind = false) const;

From 557d7d428bd450a7dc0ac6964af7f7f9129fe480 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Fri, 30 May 2014 08:28:15 -0700
Subject: [PATCH 8/9] minor rename and formatting

---
 interface/src/renderer/Model.cpp | 24 +++++++++++++++---------
 interface/src/renderer/Model.h   |  3 ++-
 2 files changed, 17 insertions(+), 10 deletions(-)

diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index b716c48603..d5006e76bc 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -42,7 +42,7 @@ Model::Model(QObject* parent) :
     _rootIndex(-1),
     _shapesAreDirty(true),
     _boundingRadius(0.0f),
-    _boundingShape(), 
+    _boundingShape(),
     _boundingShapeLocalOffset(0.0f),
     _lodDistance(0.0f),
     _pupilDilation(0.0f),
@@ -104,7 +104,7 @@ void Model::setScale(const glm::vec3& scale) {
 void Model::setScaleInternal(const glm::vec3& scale) {
     float scaleLength = glm::length(_scale);
     float relativeDeltaScale = glm::length(_scale - scale) / scaleLength;
-    
+
     const float ONE_PERCENT = 0.01f;
     if (relativeDeltaScale > ONE_PERCENT || scaleLength < EPSILON) {
         _scale = scale;
@@ -1068,13 +1068,14 @@ void Model::simulate(float deltaTime, bool fullUpdate) {
 }
 
 void Model::simulateInternal(float deltaTime) {
+    // NOTE: this is a recursive call that walks all attachments, and their attachments
+    // update the world space transforms for all joints
+
     // update animations
     foreach (const AnimationHandlePointer& handle, _runningAnimations) {
         handle->simulate(deltaTime);
     }
 
-    // NOTE: this is a recursive call that walks all attachments, and their attachments
-    // update the world space transforms for all joints
     for (int i = 0; i < _jointStates.size(); i++) {
         updateJointState(i);
     }
@@ -1847,7 +1848,7 @@ JointState::JointState() :
     _fbxJoint(NULL) {
 }
 
-void JointState::setFBXJoint(const FBXJoint* joint) { 
+void JointState::setFBXJoint(const FBXJoint* joint) {
     assert(joint != NULL);
     _rotation = joint->rotation;
     // NOTE: JointState does not own the FBXJoint to which it points.
@@ -1862,18 +1863,21 @@ void JointState::copyState(const JointState& state) {
     // DO NOT copy _fbxJoint
 }
 
-void JointState::computeTransforms(const glm::mat4& baseTransform, const glm::quat& parentRotation) {
+void JointState::computeTransforms(const glm::mat4& baseTransform, const glm::quat& baseRotation) {
     assert(_fbxJoint != NULL);
-    glm::quat combinedRotation = _fbxJoint->preRotation * _rotation * _fbxJoint->postRotation;    
-    _transform = baseTransform * glm::translate(_fbxJoint->translation) * _fbxJoint->preTransform * glm::mat4_cast(combinedRotation) * _fbxJoint->postTransform;
-    _combinedRotation = parentRotation * combinedRotation;
+    glm::quat combinedRotation = _fbxJoint->preRotation * _rotation * _fbxJoint->postRotation;
+    _transform = baseTransform * glm::translate(_fbxJoint->translation) * _fbxJoint->preTransform
+        * glm::mat4_cast(combinedRotation) * _fbxJoint->postTransform;
+    _combinedRotation = baseRotation * combinedRotation;
 }
 
 glm::quat JointState::getJointRotation(bool fromBind) const {
+    assert(_fbxJoint != NULL);
     return _combinedRotation * (fromBind ?  _fbxJoint->inverseBindRotation : _fbxJoint->inverseDefaultRotation);
 }
 
 void JointState::restoreRotation(float fraction, float priority) {
+    assert(_fbxJoint != NULL);
     if (priority == _animationPriority) {
         _rotation = safeMix(_rotation, _fbxJoint->rotation, fraction);
         _animationPriority = 0.0f;
@@ -1881,6 +1885,7 @@ void JointState::restoreRotation(float fraction, float priority) {
 }
 
 void JointState::applyRotationDelta(const glm::quat& delta, bool constrain, float priority) {
+    assert(_fbxJoint != NULL);
     if (priority < _animationPriority) {
         return;
     }
@@ -1900,5 +1905,6 @@ void JointState::applyRotationDelta(const glm::quat& delta, bool constrain, floa
 }
 
 const glm::vec3& JointState::getDefaultTranslationInParentFrame() const {
+    assert(_fbxJoint != NULL);
     return _fbxJoint->translation;
 }
diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h
index deec5aeee9..154b23fe68 100644
--- a/interface/src/renderer/Model.h
+++ b/interface/src/renderer/Model.h
@@ -40,7 +40,8 @@ public:
 
     void copyState(const JointState& state);
 
-    void computeTransforms(const glm::mat4& baseTransform, const glm::quat& parentRotation);
+    /// computes new _transform and _combinedRotation
+    void computeTransforms(const glm::mat4& baseTransform, const glm::quat& baseRotation);
 
     /// \return rotation from the joint's default (or bind) frame to world frame
     glm::quat getJointRotation(bool fromBind = false) const;

From 4eb4334ed89d5e542b2da4d7607ff97b91583b6a Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Fri, 30 May 2014 09:57:32 -0700
Subject: [PATCH 9/9] replace Model::setJointRotation() with
 JointState::setRotation()

---
 interface/src/avatar/SkeletonModel.cpp | 20 ++++++++++++--------
 interface/src/renderer/Model.cpp       | 21 ++++++++-------------
 interface/src/renderer/Model.h         |  5 ++++-
 3 files changed, 24 insertions(+), 22 deletions(-)

diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index 711e11def6..19f51d5db7 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -45,7 +45,8 @@ void SkeletonModel::simulate(float deltaTime, bool fullUpdate) {
             }
             int jointIndex = geometry.humanIKJointIndices.at(humanIKJointIndex);
             if (jointIndex != -1) {
-                setJointRotation(jointIndex, _rotation * prioVR->getJointRotations().at(i), PALM_PRIORITY);
+                JointState& state = _jointStates[jointIndex];
+                state.setRotation(_rotation * prioVR->getJointRotations().at(i), PALM_PRIORITY);
             }
         }
         return;
@@ -189,7 +190,9 @@ void SkeletonModel::applyPalmData(int jointIndex, PalmData& palm) {
         setJointPosition(parentJointIndex, palm.getPosition() + forearmVector *
             geometry.joints.at(jointIndex).distanceToParent * extractUniformScale(_scale),
             glm::quat(), false, -1, false, glm::vec3(0.0f, -1.0f, 0.0f), PALM_PRIORITY);
-        setJointRotation(parentJointIndex, palmRotation, PALM_PRIORITY);
+        JointState& parentState = _jointStates[parentJointIndex];
+        parentState.setRotation(palmRotation, PALM_PRIORITY);
+        // slam parent-relative rotation to identity
         _jointStates[jointIndex]._rotation = glm::quat();
         
     } else {
@@ -352,16 +355,17 @@ void SkeletonModel::setHandPosition(int jointIndex, const glm::vec3& position, c
     
     // ik solution
     glm::vec3 elbowPosition = shoulderPosition + glm::normalize(shoulderToWrist) * mid - tangent * height;
-    
     glm::vec3 forwardVector(rightHand ? -1.0f : 1.0f, 0.0f, 0.0f);
-    
     glm::quat shoulderRotation = rotationBetween(forwardVector, elbowPosition - shoulderPosition);
-    setJointRotation(shoulderJointIndex, shoulderRotation, PALM_PRIORITY);
+
+    JointState& shoulderState = _jointStates[shoulderJointIndex];
+    shoulderState.setRotation(shoulderRotation, PALM_PRIORITY);
     
-    setJointRotation(elbowJointIndex, rotationBetween(shoulderRotation * forwardVector,
-        wristPosition - elbowPosition) * shoulderRotation, PALM_PRIORITY);
+    JointState& elbowState = _jointStates[elbowJointIndex];
+    elbowState.setRotation(rotationBetween(shoulderRotation * forwardVector, wristPosition - elbowPosition) * shoulderRotation, PALM_PRIORITY);
     
-    setJointRotation(jointIndex, rotation, PALM_PRIORITY);
+    JointState& handState = _jointStates[jointIndex];
+    handState.setRotation(rotation, PALM_PRIORITY);
 }
     
 bool SkeletonModel::getLeftHandPosition(glm::vec3& position) const {
diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index d5006e76bc..0335378a54 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -1219,19 +1219,6 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& translation, const
     return true;
 }
 
-bool Model::setJointRotation(int jointIndex, const glm::quat& rotation, float priority) {
-    if (jointIndex == -1 || _jointStates.isEmpty()) {
-        return false;
-    }
-    JointState& state = _jointStates[jointIndex];
-    if (priority >= state._animationPriority) {
-        state._rotation = state._rotation * glm::inverse(state._combinedRotation) * rotation *
-            glm::inverse(_geometry->getFBXGeometry().joints.at(jointIndex).inverseBindRotation);
-        state._animationPriority = priority;
-    }
-    return true;
-}
-
 bool Model::restoreJointPosition(int jointIndex, float fraction, float priority) {
     if (jointIndex == -1 || _jointStates.isEmpty()) {
         return false;
@@ -1884,6 +1871,14 @@ void JointState::restoreRotation(float fraction, float priority) {
     }
 }
 
+void JointState::setRotation(const glm::quat& rotation, float priority) {
+    assert(_fbxJoint != NULL);
+    if (priority >= _animationPriority) {
+        _rotation = _rotation * glm::inverse(_combinedRotation) * rotation * glm::inverse(_fbxJoint->inverseBindRotation);
+        _animationPriority = priority;
+    }
+}
+
 void JointState::applyRotationDelta(const glm::quat& delta, bool constrain, float priority) {
     assert(_fbxJoint != NULL);
     if (priority < _animationPriority) {
diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h
index 154b23fe68..6e38c7eed4 100644
--- a/interface/src/renderer/Model.h
+++ b/interface/src/renderer/Model.h
@@ -52,6 +52,10 @@ public:
 
     void restoreRotation(float fraction, float priority);
 
+    /// \param rotation is from bind- to world-frame
+    /// computes parent relative _rotation and sets that
+    void setRotation(const glm::quat& rotation, float priority);
+
     glm::quat _rotation;     // rotation relative to parent
     glm::mat4 _transform;    // rotation to world frame + translation in model frame
     glm::quat _combinedRotation; // rotation from joint local to world frame
@@ -242,7 +246,6 @@ protected:
     bool setJointPosition(int jointIndex, const glm::vec3& translation, 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);
-    bool setJointRotation(int jointIndex, const glm::quat& rotation, float priority = 1.0f);
     
     /// 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