working on the ik dislocation caused by the offsets, head is what I am working on

interface/src/avatar/MyAvatar.cpp

@@ -431,7 +431,7 @@ void MyAvatar::reset(bool andRecenter, bool andReload, bool andHead) {
     _wasPushing = _isPushing = _isBraking = false;
     _follow.deactivate();
     if (andReload) {
-        _skeletonModel->reset();
+        //_skeletonModel->reset();
     }
     if (andHead) { // which drives camera in desktop
         getHead()->reset();

interface/src/avatar/MySkeletonModel.cpp

@@ -91,6 +91,7 @@ static AnimPose computeHipsInSensorFrame(MyAvatar* myAvatar, bool isFlying) {
 // Called within Model::simulate call, below.
 void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
     const FBXGeometry& geometry = getFBXGeometry();
+    const QMap<int, glm::quat> jointOffsetMap = _rig.getJointRotationOffsets();
 
     Head* head = _owningAvatar->getHead();
 
@@ -118,8 +119,10 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
     // input action is the highest priority source for head orientation.
     auto avatarHeadPose = myAvatar->getControllerPoseInAvatarFrame(controller::Action::HEAD);
     if (avatarHeadPose.isValid()) {
+        qCDebug(interfaceapp) << "neck joint offset " << jointOffsetMap[62];
+        AnimPose jointOffset(jointOffsetMap[62], glm::vec3());
         AnimPose pose(avatarHeadPose.getRotation(), avatarHeadPose.getTranslation());
-        params.primaryControllerPoses[Rig::PrimaryControllerType_Head] = avatarToRigPose * pose;
+        params.primaryControllerPoses[Rig::PrimaryControllerType_Head] = jointOffset.inverse() * (avatarToRigPose * pose) * jointOffset;
         params.primaryControllerFlags[Rig::PrimaryControllerType_Head] = (uint8_t)Rig::ControllerFlags::Enabled;
     } else {
         // even though full head IK is disabled, the rig still needs the head orientation to rotate the head up and
@@ -229,7 +232,8 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
         params.primaryControllerFlags[Rig::PrimaryControllerType_Hips] = (uint8_t)Rig::ControllerFlags::Enabled | (uint8_t)Rig::ControllerFlags::Estimated;
 
         // set spine2 if we have hand controllers
-        if (myAvatar->getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND).isValid() &&
+        qCDebug(interfaceapp) << "spine 2 joint offset " << jointOffsetMap[13];
+        if (false && myAvatar->getControllerPoseInAvatarFrame(controller::Action::RIGHT_HAND).isValid() &&
             myAvatar->getControllerPoseInAvatarFrame(controller::Action::LEFT_HAND).isValid() &&
             !(params.primaryControllerFlags[Rig::PrimaryControllerType_Spine2] & (uint8_t)Rig::ControllerFlags::Enabled)) {
 

libraries/animation/src/AnimSkeleton.cpp

@@ -41,19 +41,10 @@ AnimSkeleton::AnimSkeleton(const FBXGeometry& fbxGeometry, const QMap<int, glm::
             // cast into a non-const reference, so we can mutate the FBXCluster
             FBXCluster& cluster = const_cast<FBXCluster&>(mesh.clusters.at(j));
 
-            if ((cluster.jointIndex == 62) || (cluster.jointIndex == 13)) {
-                qCDebug(animation) << "cluster joint equals index " << cluster.jointIndex;
-            }
             // AJT: mutate bind pose! this allows us to oreint the skeleton back into the authored orientaiton before
             // rendering, with no runtime overhead.
             // this works if clusters match joints one for one.
-            if (cluster.jointIndex == 63) {
+          
-                //qCDebug(animation) << "Head";
-                //qCDebug(animation) << "found a joint offset to add " << cluster.jointIndex << " " << jointOffsets[cluster.jointIndex] << " cluster " << cluster.jointIndex;
-                //AnimPose localOffset(glm::quat(.7071f, 0.0f, .7071f, 0.0f), glm::vec3());
-                //cluster.inverseBindMatrix = (glm::mat4)localOffset.inverse() * cluster.inverseBindMatrix;
-                //cluster.inverseBindTransform.evalFromRawMatrix(cluster.inverseBindMatrix);
-            }
             if (cluster.jointIndex == 62) {
                 qCDebug(animation) << "Neck";
                 qCDebug(animation) << "found a joint offset to add " << cluster.jointIndex << " " << jointOffsets[cluster.jointIndex] << " cluster " << cluster.jointIndex;
@@ -246,11 +237,11 @@ void AnimSkeleton::buildSkeletonFromJoints(const std::vector<FBXJoint>& joints,
         // remember the inverse bind pose already has the offset added into it.  the total effect is offset^-1 * relDefPose * offset.
         // this gives us the correct transform for the joint that has been put in t-pose with an offset rotation.
         //relDefaultPose = relDefaultPose * _avatarTPoseOffsets[i];
-        
+
         QString jointName = getJointName(i);
         if (jointOffsets.contains(i)) {
             //QString parentIndex = getJointName(parentIndex);
-            AnimPose localOffset(jointOffsets[i], glm::vec3());
+             AnimPose localOffset(jointOffsets[i], glm::vec3());
             relDefaultPose = relDefaultPose * localOffset;
         }
         if ((parentIndex >= 0) && jointOffsets.contains(parentIndex)) {

libraries/animation/src/Rig.cpp

@@ -267,7 +267,7 @@ void Rig::initJointStates(const FBXGeometry& geometry, const glm::mat4& modelOff
     _rigToGeometryTransform = glm::inverse(_geometryToRigTransform);
     setModelOffset(modelOffset);
 
-    _animSkeleton = std::make_shared<AnimSkeleton>(geometry,_jointRotationOffsets);
+    _animSkeleton = std::make_shared<AnimSkeleton>(geometry, _jointRotationOffsets);
 
     _internalPoseSet._relativePoses.clear();
     _internalPoseSet._relativePoses = _animSkeleton->getRelativeDefaultPoses();
@@ -2084,4 +2084,8 @@ void Rig::setJointRotationOffsets(const QMap<QString, glm::quat>& offsets) {
         _jointRotationOffsets.insert(spine2Id, glm::quat(0.5f, 0.5f, 0.5f, -0.5f));
     }
     qCDebug(animation) << "set the neck and spine2 offsets " << spine2Id << " " << neckId;
+}
+
+const QMap<int, glm::quat>& Rig::getJointRotationOffsets() const {
+    return _jointRotationOffsets;
 }

libraries/animation/src/Rig.h

@@ -128,6 +128,7 @@ public:
     int getJointStateCount() const;
     int indexOfJoint(const QString& jointName) const;
     QString nameOfJoint(int jointIndex) const;
+    const QMap<int, glm::quat>& getJointRotationOffsets() const;
 
     void setModelOffset(const glm::mat4& modelOffsetMat);
 
@@ -422,6 +423,7 @@ protected:
 
     SnapshotBlendPoseHelper _hipsBlendHelper;
     ControllerParameters _previousControllerParameters;
+    bool _alreadyInitialized { false };
 };
 
 #endif /* defined(__hifi__Rig__) */