Rig: Switched over to use AnimPoses instead of JointStates

* fixed debug rendering
* improved jointState/animPose diff detection code.

interface/src/avatar/MyAvatar.cpp

@@ -660,7 +660,7 @@ void MyAvatar::setEnableDebugDrawAnimPose(bool isEnabled) {
     _enableDebugDrawAnimPose = isEnabled;
 
     if (!isEnabled) {
-        AnimDebugDraw::getInstance().removePoses("myAvatar");
+        AnimDebugDraw::getInstance().removeAbsolutePoses("myAvatar");
     }
 }
 
@@ -1278,37 +1278,25 @@ void MyAvatar::preRender(RenderArgs* renderArgs) {
         glm::quat rotY180 = glm::angleAxis((float)M_PI, glm::vec3(0.0f, 1.0f, 0.0f));
         AnimPose xform(glm::vec3(1), getOrientation() * rotY180, getPosition());
 
+        /*
         if (_enableDebugDrawBindPose && _debugDrawSkeleton) {
             glm::vec4 gray(0.2f, 0.2f, 0.2f, 0.2f);
             AnimDebugDraw::getInstance().addSkeleton("myAvatar", _debugDrawSkeleton, xform, gray);
         }
+        */
 
         if (_enableDebugDrawAnimPose && _debugDrawSkeleton) {
             glm::vec4 cyan(0.1f, 0.6f, 0.6f, 1.0f);
 
             auto rig = _skeletonModel.getRig();
-            // AJT: TODO move this into rig!
-            // build AnimPoseVec from JointStates.
-            // AJT: TODO THIS SHIT IS ALL BROKEN
-            AnimPoseVec poses;
-            poses.reserve(_debugDrawSkeleton->getNumJoints());
-            for (int i = 0; i < _debugDrawSkeleton->getNumJoints(); i++) {
-                AnimPose pose = _debugDrawSkeleton->getRelativeBindPose(i);
-                glm::quat jointRot;
-                _rig->getJointRotationInConstrainedFrame(i, jointRot);
-                glm::vec3 jointTrans;
-                _rig->getJointTranslation(i, jointTrans);
-                pose.rot = pose.rot * jointRot;
-                pose.trans = jointTrans;
-                /*
-                if (_debugDrawSkeleton->getParentIndex(i) < 0) {
-                    pose = _rig->getGeometryOffset() * pose;
-                }
-                */
-                poses.push_back(pose);
-            }
 
-            AnimDebugDraw::getInstance().addPoses("myAvatar", _debugDrawSkeleton, poses, xform, cyan);
+            // build AnimPoseVec from JointStates.
+            AnimPoseVec absPoses;
+            absPoses.reserve(_debugDrawSkeleton->getNumJoints());
+            for (int i = 0; i < _rig->getJointStateCount(); i++) {
+                absPoses.push_back(AnimPose(_rig->getJointTransform(i)));
+            }
+            AnimDebugDraw::getInstance().addAbsolutePoses("myAvatar", _debugDrawSkeleton, absPoses, xform, cyan);
         }
     }
 
@@ -1789,6 +1777,9 @@ glm::mat4 MyAvatar::deriveBodyFromHMDSensor() const {
         int neckIndex = _debugDrawSkeleton->nameToJointIndex("Neck");
         int hipsIndex = _debugDrawSkeleton->nameToJointIndex("Hips");
 
+        // AJT: TODO: perhaps expose default gets from rig?
+        // so this can become _rig->getAbsoluteDefaultPose(rightEyeIndex)...
+
         glm::vec3 absRightEyePos = rightEyeIndex != -1 ? geometryOffset * _debugDrawSkeleton->getAbsoluteBindPose(rightEyeIndex).trans : DEFAULT_RIGHT_EYE_POS;
         glm::vec3 absLeftEyePos = leftEyeIndex != -1 ? geometryOffset * _debugDrawSkeleton->getAbsoluteBindPose(leftEyeIndex).trans : DEFAULT_LEFT_EYE_POS;
         glm::vec3 absNeckPos = neckIndex != -1 ? geometryOffset * _debugDrawSkeleton->getAbsoluteBindPose(neckIndex).trans : DEFAULT_NECK_POS;

interface/src/avatar/SkeletonModel.cpp

@@ -444,7 +444,7 @@ bool SkeletonModel::getNeckParentRotationFromDefaultOrientation(glm::quat& neckP
 bool SkeletonModel::getEyeModelPositions(glm::vec3& firstEyePosition, glm::vec3& secondEyePosition) const {
     if (!isActive()) {
         return false;
-        }
+    }
     const FBXGeometry& geometry = _geometry->getFBXGeometry();
     if (getJointPosition(geometry.leftEyeJointIndex, firstEyePosition) &&
         getJointPosition(geometry.rightEyeJointIndex, secondEyePosition)) {

libraries/animation/src/AnimPose.cpp

@@ -19,7 +19,8 @@ const AnimPose AnimPose::identity = AnimPose(glm::vec3(1.0f),
 
 AnimPose::AnimPose(const glm::mat4& mat) {
     scale = extractScale(mat);
-    if (std::max(std::max(scale.x, scale.y), scale.z) > 1.01f) {
+    float maxScale = std::max(std::max(scale.x, scale.y), scale.z);
+    if (maxScale > 1.01f || maxScale <= 0.99f) {
         // quat_cast doesn't work so well with scaled matrices, so cancel it out.
         mat4 tmp = glm::scale(mat, 1.0f / scale);
         rot = glm::normalize(glm::quat_cast(tmp));

libraries/animation/src/Rig.cpp

@@ -167,6 +167,7 @@ void Rig::initJointStates(const FBXGeometry& geometry, glm::mat4 modelOffset, in
                           int leftHandJointIndex, int leftElbowJointIndex, int leftShoulderJointIndex,
                           int rightHandJointIndex, int rightElbowJointIndex, int rightShoulderJointIndex) {
 
+    setModelOffset(modelOffset);
     _geometryOffset = AnimPose(geometry.offset);
     _animSkeleton = std::make_shared<AnimSkeleton>(geometry);
 
@@ -221,8 +222,6 @@ void Rig::initJointStates(const FBXGeometry& geometry, glm::mat4 modelOffset, in
     _rightHandJointIndex = rightHandJointIndex;
     _rightElbowJointIndex = rightElbowJointIndex;
     _rightShoulderJointIndex = rightShoulderJointIndex;
-
-    setModelOffset(modelOffset);
 }
 
 bool Rig::jointStatesEmpty() {
@@ -732,6 +731,8 @@ void Rig::updateAnimationStateHandlers() { // called on avatar update thread (wh
 
 void Rig::updateAnimations(float deltaTime, glm::mat4 rootTransform) {
 
+    setModelOffset(rootTransform);
+
     if (_animNode) {
 
         updateAnimationStateHandlers();
@@ -763,7 +764,7 @@ void Rig::updateAnimations(float deltaTime, glm::mat4 rootTransform) {
         computeEyesInRootFrame(_relativePoses);
     }
 
-    setModelOffset(rootTransform);
+    // AJT: enable this for script
     //applyOverridePoses();
     buildAbsolutePoses();
 
@@ -1263,8 +1264,9 @@ void Rig::buildAbsolutePoses() {
         }
     }
 
+    AnimPose rootTransform(_modelOffset * _geometryOffset);
     for (int i = 0; i < (int)_absolutePoses.size(); i++) {
-        _absolutePoses[i] = _modelOffset * _geometryOffset * _absolutePoses[i];
+        _absolutePoses[i] = rootTransform * _absolutePoses[i];
         // AJT: REMOVE
         /*
         _absolutePoses[i].trans = _modelOffset.trans + _absolutePoses[i].trans;
@@ -1306,29 +1308,57 @@ glm::mat4 Rig::getJointTransform(int jointIndex) const {
 
     // check for differences between jointStates and _absolutePoses!
     if (false) {
-        // should display for model entities
+
-        glm::mat4 newMat = _absolutePoses[jointIndex];
         glm::mat4 oldMat = _jointStates[jointIndex].getTransform();
+        AnimPose oldPose(oldMat);
 
-        const float EPSILON = 0.01f;
+        glm::mat4 newMat = _absolutePoses[jointIndex];
-        if (glm::length(newMat[0] - oldMat[0]) > EPSILON ||
+        AnimPose newPose(newMat);
-            glm::length(newMat[1] - oldMat[1]) > EPSILON ||
-            glm::length(newMat[2] - oldMat[2]) > EPSILON ||
-            glm::length(newMat[3] - oldMat[3]) > EPSILON) {
 
-            // error?
+        bool badTrans = false;
+        const float TRANS_EPSILON = 0.01f;
+        if (glm::length(newPose.trans - oldPose.trans) > TRANS_EPSILON) {
+            badTrans = true;
+        }
+
+        bool badScale = false;
+        const float SCALE_EPSILON = 0.00001f;
+        if (glm::length(newPose.scale - oldPose.scale) > SCALE_EPSILON) {
+            badScale = true;
+        }
+
+        bool badRot = false;
+        const float ROT_EPSILON = 0.0001f;
+        glm::quat oldLog = glm::log(newPose.rot);
+        glm::quat newLog = glm::log(oldPose.rot);
+        glm::vec3 oldLogVec(oldLog.x, oldLog.y, oldLog.z);
+        glm::vec3 newLogVec(newLog.x, newLog.y, newLog.z);
+        if (glm::length(oldLogVec - newLogVec) > ROT_EPSILON) {
+            badRot = true;
+        }
+
+        if (badTrans || badScale || badRot) {
             qCDebug(animation).nospace() << "AJT: mismatch for " << _animSkeleton->getJointName(jointIndex) << ", joint[" << jointIndex << "]";
-            qCDebug(animation) << "AJT: oldMat = " << AnimPose(oldMat);
+            if (badTrans) {
-            qCDebug(animation) << "AJT: newMat = " << AnimPose(newMat);
+                qCDebug(animation) << "AJT: oldTrans = " << oldPose.trans;
-
+                qCDebug(animation) << "AJT: newTrans = " << newPose.trans;
+            }
+            if (badRot) {
+                qCDebug(animation) << "AJT: oldRot = " << oldPose.rot << "log =" << glm::log(oldPose.rot);
+                qCDebug(animation) << "AJT: newRot = " << newPose.rot << "log =" << glm::log(newPose.rot);
+            }
+            if (badScale) {
+                qCDebug(animation) << "AJT: oldScale = " << oldPose.scale;
+                qCDebug(animation) << "AJT: newScale = " << newPose.scale;
+            }
         }
     }
 
     // AJT: LEGACY
     {
-        return _jointStates[jointIndex].getTransform();
+        //return _jointStates[jointIndex].getTransform();
     }
 
-    //return _absolutePoses[jointIndex];
+    return _absolutePoses[jointIndex];
 }
 

libraries/render-utils/src/AnimDebugDraw.cpp

@@ -169,12 +169,20 @@ void AnimDebugDraw::removeAnimNode(const std::string& key) {
     _animNodes.erase(key);
 }
 
-void AnimDebugDraw::addPoses(const std::string& key, AnimSkeleton::ConstPointer skeleton, const AnimPoseVec& poses, const AnimPose& rootPose, const glm::vec4& color) {
+void AnimDebugDraw::addRelativePoses(const std::string& key, AnimSkeleton::ConstPointer skeleton, const AnimPoseVec& poses, const AnimPose& rootPose, const glm::vec4& color) {
-    _poses[key] = PosesInfo(skeleton, poses, rootPose, color);
+    _relativePoses[key] = PosesInfo(skeleton, poses, rootPose, color);
 }
 
-void AnimDebugDraw::removePoses(const std::string& key) {
+void AnimDebugDraw::removeRelativePoses(const std::string& key) {
-    _poses.erase(key);
+    _relativePoses.erase(key);
+}
+
+void AnimDebugDraw::addAbsolutePoses(const std::string& key, AnimSkeleton::ConstPointer skeleton, const AnimPoseVec& poses, const AnimPose& rootPose, const glm::vec4& color) {
+    _absolutePoses[key] = PosesInfo(skeleton, poses, rootPose, color);
+}
+
+void AnimDebugDraw::removeAbsolutePoses(const std::string& key) {
+    _absolutePoses.erase(key);
 }
 
 static const uint32_t red = toRGBA(255, 0, 0, 255);
@@ -338,6 +346,9 @@ void AnimDebugDraw::update() {
 
         // figure out how many verts we will need.
         int numVerts = 0;
+
+        // AJT: FIX ME
+        /*
         for (auto& iter : _skeletons) {
             AnimSkeleton::ConstPointer& skeleton = std::get<0>(iter.second);
             numVerts += skeleton->getNumJoints() * VERTICES_PER_BONE;
@@ -362,7 +373,19 @@ void AnimDebugDraw::update() {
             }
         }
 
-        for (auto& iter : _poses) {
+        for (auto& iter : _relativePoses) {
+            AnimSkeleton::ConstPointer& skeleton = std::get<0>(iter.second);
+            numVerts += skeleton->getNumJoints() * VERTICES_PER_BONE;
+            for (int i = 0; i < skeleton->getNumJoints(); i++) {
+                auto parentIndex = skeleton->getParentIndex(i);
+                if (parentIndex >= 0) {
+                    numVerts += VERTICES_PER_LINK;
+                }
+            }
+        }
+        */
+
+        for (auto& iter : _absolutePoses) {
             AnimSkeleton::ConstPointer& skeleton = std::get<0>(iter.second);
             numVerts += skeleton->getNumJoints() * VERTICES_PER_BONE;
             for (int i = 0; i < skeleton->getNumJoints(); i++) {
@@ -379,9 +402,14 @@ void AnimDebugDraw::update() {
         auto myAvatarMarkerMap = DebugDraw::getInstance().getMyAvatarMarkerMap();
         numVerts += myAvatarMarkerMap.size() * VERTICES_PER_BONE;
 
+        // allocate verts!
         data._vertexBuffer->resize(sizeof(Vertex) * numVerts);
         Vertex* verts = (Vertex*)data._vertexBuffer->editData();
         Vertex* v = verts;
+
+        // AJT: fixme
+        // draw skeletons
+        /*
         for (auto& iter : _skeletons) {
             AnimSkeleton::ConstPointer& skeleton = std::get<0>(iter.second);
             AnimPose rootPose = std::get<1>(iter.second);
@@ -408,7 +436,11 @@ void AnimDebugDraw::update() {
                 }
             }
         }
+        */
 
+        // AJT: FIXME
+        /*
+        // draw animNodes
         for (auto& iter : _animNodes) {
             AnimNode::ConstPointer& animNode = std::get<0>(iter.second);
             AnimPose rootPose = std::get<1>(iter.second);
@@ -446,8 +478,12 @@ void AnimDebugDraw::update() {
                 }
             }
         }
+        */
 
-        for (auto& iter : _poses) {
+        // AJT: FIX ME
+        /*
+        // draw relative poses
+        for (auto& iter : _relativePoses) {
             AnimSkeleton::ConstPointer& skeleton = std::get<0>(iter.second);
             AnimPoseVec& poses = std::get<1>(iter.second);
             AnimPose rootPose = std::get<2>(iter.second);
@@ -482,6 +518,29 @@ void AnimDebugDraw::update() {
                 }
             }
         }
+        */
+
+        // draw absolute poses
+        for (auto& iter : _absolutePoses) {
+            AnimSkeleton::ConstPointer& skeleton = std::get<0>(iter.second);
+            AnimPoseVec& absPoses = std::get<1>(iter.second);
+            AnimPose rootPose = std::get<2>(iter.second);
+            glm::vec4 color = std::get<3>(iter.second);
+
+            for (int i = 0; i < skeleton->getNumJoints(); i++) {
+                const float radius = BONE_RADIUS / (absPoses[i].scale.x * rootPose.scale.x);
+
+                // draw bone
+                addBone(rootPose, absPoses[i], radius, v);
+
+                // draw link to parent
+                auto parentIndex = skeleton->getParentIndex(i);
+                if (parentIndex >= 0) {
+                    assert(parentIndex < skeleton->getNumJoints());
+                    addLink(rootPose, absPoses[i], absPoses[parentIndex], radius, color, v);
+                }
+            }
+        }
 
         // draw markers from shared DebugDraw singleton
         for (auto& iter : markerMap) {

libraries/render-utils/src/AnimDebugDraw.h

@@ -38,8 +38,11 @@ public:
     void removeAnimNode(const std::string& key);
 
     // draw a set of poses with a skeleton
-    void addPoses(const std::string& key, AnimSkeleton::ConstPointer skeleton, const AnimPoseVec& poses, const AnimPose& rootPose, const glm::vec4& color);
+    void addRelativePoses(const std::string& key, AnimSkeleton::ConstPointer skeleton, const AnimPoseVec& poses, const AnimPose& rootPose, const glm::vec4& color);
-    void removePoses(const std::string& key);
+    void removeRelativePoses(const std::string& key);
+
+    void addAbsolutePoses(const std::string& key, AnimSkeleton::ConstPointer skeleton, const AnimPoseVec& poses, const AnimPose& rootPose, const glm::vec4& color);
+    void removeAbsolutePoses(const std::string& key);
 
     void update();
 
@@ -56,7 +59,8 @@ protected:
 
     std::unordered_map<std::string, SkeletonInfo> _skeletons;
     std::unordered_map<std::string, AnimNodeInfo> _animNodes;
-    std::unordered_map<std::string, PosesInfo> _poses;
+    std::unordered_map<std::string, PosesInfo> _relativePoses;
+    std::unordered_map<std::string, PosesInfo> _absolutePoses;
 
     // no copies
     AnimDebugDraw(const AnimDebugDraw&) = delete;