Merge pull request #5724 from hyperlogic/tony/anim-debug-draw-improvements

AnimDebugDraw Improvements

interface/src/avatar/MyAvatar.cpp

@@ -735,7 +735,7 @@ void MyAvatar::setEnableDebugDrawAnimPose(bool isEnabled) {
     _enableDebugDrawAnimPose = isEnabled;
 
     if (!isEnabled) {
-        AnimDebugDraw::getInstance().removeAnimNode("myAvatar");
+        AnimDebugDraw::getInstance().removePoses("myAvatar");
     }
 }
 
@@ -1248,8 +1248,6 @@ void MyAvatar::initAnimGraph() {
 
 void MyAvatar::destroyAnimGraph() {
     _rig->destroyAnimGraph();
-    AnimDebugDraw::getInstance().removeSkeleton("myAvatar");
-    AnimDebugDraw::getInstance().removeAnimNode("myAvatar");
 }
 
 void MyAvatar::preRender(RenderArgs* renderArgs) {
@@ -1261,26 +1259,35 @@ void MyAvatar::preRender(RenderArgs* renderArgs) {
         initHeadBones();
         _skeletonModel.setCauterizeBoneSet(_headBoneSet);
         initAnimGraph();
+        _debugDrawSkeleton = std::make_shared<AnimSkeleton>(_skeletonModel.getGeometry()->getFBXGeometry());
     }
 
     if (_enableDebugDrawBindPose || _enableDebugDrawAnimPose) {
 
-        AnimSkeleton::ConstPointer animSkeleton = _rig->getAnimSkeleton();
-        AnimNode::ConstPointer animNode = _rig->getAnimNode();
-
-        // bones space is rotated
+        // bones are aligned such that z is forward, not -z.
         glm::quat rotY180 = glm::angleAxis((float)M_PI, glm::vec3(0.0f, 1.0f, 0.0f));
         AnimPose xform(glm::vec3(1), rotY180 * getOrientation(), getPosition());
 
-        if (animSkeleton && _enableDebugDrawBindPose) {
+        if (_enableDebugDrawBindPose && _debugDrawSkeleton) {
             glm::vec4 gray(0.2f, 0.2f, 0.2f, 0.2f);
-            AnimDebugDraw::getInstance().addSkeleton("myAvatar", animSkeleton, xform, gray);
+            AnimDebugDraw::getInstance().addSkeleton("myAvatar", _debugDrawSkeleton, xform, gray);
         }
 
-        // This only works for when new anim system is enabled, at the moment.
-        if (animNode && animSkeleton && _enableDebugDrawAnimPose && _rig->getEnableAnimGraph()) {
+        if (_enableDebugDrawAnimPose && _debugDrawSkeleton) {
             glm::vec4 cyan(0.1f, 0.6f, 0.6f, 1.0f);
-            AnimDebugDraw::getInstance().addAnimNode("myAvatar", animNode, xform, cyan);
+
+            // build AnimPoseVec from JointStates.
+            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);
+                pose.rot = pose.rot * jointRot;
+                poses.push_back(pose);
+            }
+
+            AnimDebugDraw::getInstance().addPoses("myAvatar", _debugDrawSkeleton, poses, xform, cyan);
         }
     }
 

interface/src/avatar/MyAvatar.h

@@ -321,6 +321,7 @@ private:
 
     bool _enableDebugDrawBindPose = false;
     bool _enableDebugDrawAnimPose = false;
+    AnimSkeleton::ConstPointer _debugDrawSkeleton = nullptr;
 };
 
 #endif // hifi_MyAvatar_h

libraries/animation/src/AnimSkeleton.cpp

@@ -58,7 +58,52 @@ AnimPose::operator glm::mat4() const {
                      glm::vec4(zAxis, 0.0f), glm::vec4(trans, 1.0f));
 }
 
+AnimSkeleton::AnimSkeleton(const FBXGeometry& fbxGeometry) {
+    // convert to std::vector of joints
+    std::vector<FBXJoint> joints;
+    joints.reserve(fbxGeometry.joints.size());
+    for (auto& joint : fbxGeometry.joints) {
+        joints.push_back(joint);
+    }
+
+    AnimPose geometryOffset(fbxGeometry.offset);
+    buildSkeletonFromJoints(joints, geometryOffset);
+}
+
 AnimSkeleton::AnimSkeleton(const std::vector<FBXJoint>& joints, const AnimPose& geometryOffset) {
+    buildSkeletonFromJoints(joints, geometryOffset);
+}
+
+int AnimSkeleton::nameToJointIndex(const QString& jointName) const {
+    for (size_t i = 0; i < _joints.size(); i++) {
+        if (_joints[i].name == jointName) {
+            return i;
+        }
+    }
+    return -1;
+}
+
+int AnimSkeleton::getNumJoints() const {
+    return _joints.size();
+}
+
+const AnimPose& AnimSkeleton::getAbsoluteBindPose(int jointIndex) const {
+    return _absoluteBindPoses[jointIndex];
+}
+
+const AnimPose& AnimSkeleton::getRelativeBindPose(int jointIndex) const {
+    return _relativeBindPoses[jointIndex];
+}
+
+int AnimSkeleton::getParentIndex(int jointIndex) const {
+    return _joints[jointIndex].parentIndex;
+}
+
+const QString& AnimSkeleton::getJointName(int jointIndex) const {
+    return _joints[jointIndex].name;
+}
+
+void AnimSkeleton::buildSkeletonFromJoints(const std::vector<FBXJoint>& joints, const AnimPose& geometryOffset) {
     _joints = joints;
 
     // build a cache of bind poses
@@ -113,35 +158,6 @@ AnimSkeleton::AnimSkeleton(const std::vector<FBXJoint>& joints, const AnimPose&
     }
 }
 
-int AnimSkeleton::nameToJointIndex(const QString& jointName) const {
-    for (size_t i = 0; i < _joints.size(); i++) {
-        if (_joints[i].name == jointName) {
-            return i;
-        }
-    }
-    return -1;
-}
-
-int AnimSkeleton::getNumJoints() const {
-    return _joints.size();
-}
-
-AnimPose AnimSkeleton::getAbsoluteBindPose(int jointIndex) const {
-    return _absoluteBindPoses[jointIndex];
-}
-
-AnimPose AnimSkeleton::getRelativeBindPose(int jointIndex) const {
-    return _relativeBindPoses[jointIndex];
-}
-
-int AnimSkeleton::getParentIndex(int jointIndex) const {
-    return _joints[jointIndex].parentIndex;
-}
-
-const QString& AnimSkeleton::getJointName(int jointIndex) const {
-    return _joints[jointIndex].name;
-}
-
 #ifndef NDEBUG
 void AnimSkeleton::dump() const {
     qCDebug(animation) << "[";

libraries/animation/src/AnimSkeleton.h

@@ -47,16 +47,17 @@ public:
     using Pointer = std::shared_ptr<AnimSkeleton>;
     using ConstPointer = std::shared_ptr<const AnimSkeleton>;
 
+    AnimSkeleton(const FBXGeometry& fbxGeometry);
     AnimSkeleton(const std::vector<FBXJoint>& joints, const AnimPose& geometryOffset);
     int nameToJointIndex(const QString& jointName) const;
     const QString& getJointName(int jointIndex) const;
     int getNumJoints() const;
 
     // absolute pose, not relative to parent
-    AnimPose getAbsoluteBindPose(int jointIndex) const;
+    const AnimPose& getAbsoluteBindPose(int jointIndex) const;
 
     // relative to parent pose
-    AnimPose getRelativeBindPose(int jointIndex) const;
+    const AnimPose& getRelativeBindPose(int jointIndex) const;
 
     int getParentIndex(int jointIndex) const;
 
@@ -66,6 +67,8 @@ public:
 #endif
 
 protected:
+    void buildSkeletonFromJoints(const std::vector<FBXJoint>& joints, const AnimPose& geometryOffset);
+
     std::vector<FBXJoint> _joints;
     AnimPoseVec _absoluteBindPoses;
     AnimPoseVec _relativeBindPoses;

libraries/animation/src/Rig.cpp

@@ -901,6 +901,14 @@ glm::quat Rig::setJointRotationInConstrainedFrame(int jointIndex, glm::quat targ
     return endRotation;
 }
 
+bool Rig::getJointRotationInConstrainedFrame(int jointIndex, glm::quat& quatOut) const {
+    if (jointIndex == -1 || _jointStates.isEmpty()) {
+        return false;
+    }
+    quatOut = _jointStates[jointIndex].getRotationInConstrainedFrame();
+    return true;
+}
+
 void Rig::updateVisibleJointStates() {
     for (int i = 0; i < _jointStates.size(); i++) {
         _jointStates[i].slaveVisibleTransform();
@@ -1010,16 +1018,7 @@ void Rig::initAnimGraph(const QUrl& url, const FBXGeometry& fbxGeometry) {
         return;
     }
 
-    // convert to std::vector of joints
-    std::vector<FBXJoint> joints;
-    joints.reserve(fbxGeometry.joints.size());
-    for (auto& joint : fbxGeometry.joints) {
-        joints.push_back(joint);
-    }
-
-    // create skeleton
-    AnimPose geometryOffset(fbxGeometry.offset);
-    _animSkeleton = std::make_shared<AnimSkeleton>(joints, geometryOffset);
+    _animSkeleton = std::make_shared<AnimSkeleton>(fbxGeometry);
 
     // load the anim graph
     _animLoader.reset(new AnimNodeLoader(url));

libraries/animation/src/Rig.h

@@ -50,7 +50,6 @@ class Rig;
 typedef std::shared_ptr<Rig> RigPointer;
 
 class Rig : public QObject, public std::enable_shared_from_this<Rig> {
-
 public:
 
     struct HeadParameters {
@@ -153,6 +152,7 @@ public:
     glm::vec3 getJointDefaultTranslationInConstrainedFrame(int jointIndex);
     glm::quat setJointRotationInConstrainedFrame(int jointIndex, glm::quat targetRotation,
                                                  float priority, bool constrain = false, float mix = 1.0f);
+    bool getJointRotationInConstrainedFrame(int jointIndex, glm::quat& rotOut) const;
     glm::quat getJointDefaultRotationInParentFrame(int jointIndex);
     void updateVisibleJointStates();
 

libraries/render-utils/src/AnimDebugDraw.cpp

@@ -152,22 +152,30 @@ AnimDebugDraw::~AnimDebugDraw() {
     }
 }
 
-void AnimDebugDraw::addSkeleton(std::string key, AnimSkeleton::ConstPointer skeleton, const AnimPose& rootPose, const glm::vec4& color) {
+void AnimDebugDraw::addSkeleton(const std::string& key, AnimSkeleton::ConstPointer skeleton, const AnimPose& rootPose, const glm::vec4& color) {
     _skeletons[key] = SkeletonInfo(skeleton, rootPose, color);
 }
 
-void AnimDebugDraw::removeSkeleton(std::string key) {
+void AnimDebugDraw::removeSkeleton(const std::string& key) {
     _skeletons.erase(key);
 }
 
-void AnimDebugDraw::addAnimNode(std::string key, AnimNode::ConstPointer animNode, const AnimPose& rootPose, const glm::vec4& color) {
+void AnimDebugDraw::addAnimNode(const std::string& key, AnimNode::ConstPointer animNode, const AnimPose& rootPose, const glm::vec4& color) {
     _animNodes[key] = AnimNodeInfo(animNode, rootPose, color);
 }
 
-void AnimDebugDraw::removeAnimNode(std::string key) {
+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) {
+    _poses[key] = PosesInfo(skeleton, poses, rootPose, color);
+}
+
+void AnimDebugDraw::removePoses(const std::string& key) {
+    _poses.erase(key);
+}
+
 static const uint32_t red = toRGBA(255, 0, 0, 255);
 static const uint32_t green = toRGBA(0, 255, 0, 255);
 static const uint32_t blue = toRGBA(0, 0, 255, 255);
@@ -176,6 +184,8 @@ const int NUM_CIRCLE_SLICES = 24;
 
 static void addBone(const AnimPose& rootPose, const AnimPose& pose, float radius, Vertex*& v) {
 
+    const float XYZ_AXIS_LENGTH = radius * 4.0f;
+
     AnimPose finalPose = rootPose * pose;
     glm::vec3 base = rootPose * pose.trans;
 
@@ -195,7 +205,7 @@ static void addBone(const AnimPose& rootPose, const AnimPose& pose, float radius
     v->pos = base;
     v->rgba = red;
     v++;
-    v->pos = finalPose * glm::vec3(radius * 2.0f, 0.0f, 0.0f);
+    v->pos = finalPose * glm::vec3(XYZ_AXIS_LENGTH, 0.0f, 0.0f);
     v->rgba = red;
     v++;
 
@@ -213,7 +223,7 @@ static void addBone(const AnimPose& rootPose, const AnimPose& pose, float radius
     v->pos = base;
     v->rgba = green;
     v++;
-    v->pos = finalPose * glm::vec3(0.0f, radius * 2.0f, 0.0f);
+    v->pos = finalPose * glm::vec3(0.0f, XYZ_AXIS_LENGTH, 0.0f);
     v->rgba = green;
     v++;
 
@@ -231,7 +241,7 @@ static void addBone(const AnimPose& rootPose, const AnimPose& pose, float radius
     v->pos = base;
     v->rgba = blue;
     v++;
-    v->pos = finalPose * glm::vec3(0.0f, 0.0f, radius * 2.0f);
+    v->pos = finalPose * glm::vec3(0.0f, 0.0f, XYZ_AXIS_LENGTH);
     v->rgba = blue;
     v++;
 
@@ -322,11 +332,11 @@ void AnimDebugDraw::update() {
         const size_t VERTICES_PER_BONE = (6 + (NUM_CIRCLE_SLICES * 2) * 3);
         const size_t VERTICES_PER_LINK = 8 * 2;
 
-        const float BONE_RADIUS = 0.0075f;
+        const float BONE_RADIUS = 0.01f; // 1 cm
 
         // figure out how many verts we will need.
         int numVerts = 0;
-        for (auto&& iter : _skeletons) {
+        for (auto& iter : _skeletons) {
             AnimSkeleton::ConstPointer& skeleton = std::get<0>(iter.second);
             numVerts += skeleton->getNumJoints() * VERTICES_PER_BONE;
             for (int i = 0; i < skeleton->getNumJoints(); i++) {
@@ -337,7 +347,7 @@ void AnimDebugDraw::update() {
             }
         }
 
-        for (auto&& iter : _animNodes) {
+        for (auto& iter : _animNodes) {
             AnimNode::ConstPointer& animNode = std::get<0>(iter.second);
             auto poses = animNode->getPosesInternal();
             numVerts += poses.size() * VERTICES_PER_BONE;
@@ -350,10 +360,21 @@ void AnimDebugDraw::update() {
             }
         }
 
+        for (auto& iter : _poses) {
+            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;
+                }
+            }
+        }
+
         data._vertexBuffer->resize(sizeof(Vertex) * numVerts);
         Vertex* verts = (Vertex*)data._vertexBuffer->editData();
         Vertex* v = verts;
-        for (auto&& iter : _skeletons) {
+        for (auto& iter : _skeletons) {
             AnimSkeleton::ConstPointer& skeleton = std::get<0>(iter.second);
             AnimPose rootPose = std::get<1>(iter.second);
             int hipsIndex = skeleton->nameToJointIndex("Hips");
@@ -380,7 +401,7 @@ void AnimDebugDraw::update() {
             }
         }
 
-        for (auto&& iter : _animNodes) {
+        for (auto& iter : _animNodes) {
             AnimNode::ConstPointer& animNode = std::get<0>(iter.second);
             AnimPose rootPose = std::get<1>(iter.second);
             if (animNode->_skeleton) {
@@ -418,6 +439,42 @@ void AnimDebugDraw::update() {
             }
         }
 
+        for (auto& iter : _poses) {
+            AnimSkeleton::ConstPointer& skeleton = std::get<0>(iter.second);
+            AnimPoseVec& poses = std::get<1>(iter.second);
+            AnimPose rootPose = std::get<2>(iter.second);
+            int hipsIndex = skeleton->nameToJointIndex("Hips");
+            if (hipsIndex >= 0) {
+                rootPose.trans -= skeleton->getRelativeBindPose(hipsIndex).trans;
+            }
+            glm::vec4 color = std::get<3>(iter.second);
+
+            std::vector<AnimPose> absAnimPose;
+            absAnimPose.resize(skeleton->getNumJoints());
+
+            for (int i = 0; i < skeleton->getNumJoints(); i++) {
+                const AnimPose& pose = poses[i];
+
+                const float radius = BONE_RADIUS / (pose.scale.x * rootPose.scale.x);
+
+                auto parentIndex = skeleton->getParentIndex(i);
+                if (parentIndex >= 0) {
+                    absAnimPose[i] = absAnimPose[parentIndex] * pose;
+                } else {
+                    absAnimPose[i] = pose;
+                }
+
+                // draw bone
+                addBone(rootPose, absAnimPose[i], radius, v);
+
+                // draw link to parent
+                if (parentIndex >= 0) {
+                    assert(parentIndex < skeleton->getNumJoints());
+                    addLink(rootPose, absAnimPose[i], absAnimPose[parentIndex], radius, color, v);
+                }
+            }
+        }
+
         assert(numVerts == (v - verts));
 
         data._indexBuffer->resize(sizeof(uint16_t) * numVerts);

libraries/render-utils/src/AnimDebugDraw.h

@@ -27,11 +27,14 @@ public:
     AnimDebugDraw();
     ~AnimDebugDraw();
 
-    void addSkeleton(std::string key, AnimSkeleton::ConstPointer skeleton, const AnimPose& rootPose, const glm::vec4& color);
-    void removeSkeleton(std::string key);
+    void addSkeleton(const std::string& key, AnimSkeleton::ConstPointer skeleton, const AnimPose& rootPose, const glm::vec4& color);
+    void removeSkeleton(const std::string& key);
 
-    void addAnimNode(std::string key, AnimNode::ConstPointer animNode, const AnimPose& rootPose, const glm::vec4& color);
-    void removeAnimNode(std::string key);
+    void addAnimNode(const std::string& key, AnimNode::ConstPointer animNode, const AnimPose& rootPose, const glm::vec4& color);
+    void removeAnimNode(const std::string& key);
+
+    void addPoses(const std::string& key, AnimSkeleton::ConstPointer skeleton, const AnimPoseVec& poses, const AnimPose& rootPose, const glm::vec4& color);
+    void removePoses(const std::string& key);
 
     void update();
 
@@ -44,9 +47,11 @@ protected:
 
     typedef std::tuple<AnimSkeleton::ConstPointer, AnimPose, glm::vec4> SkeletonInfo;
     typedef std::tuple<AnimNode::ConstPointer, AnimPose, glm::vec4> AnimNodeInfo;
+    typedef std::tuple<AnimSkeleton::ConstPointer, AnimPoseVec, AnimPose, glm::vec4> PosesInfo;
 
     std::unordered_map<std::string, SkeletonInfo> _skeletons;
     std::unordered_map<std::string, AnimNodeInfo> _animNodes;
+    std::unordered_map<std::string, PosesInfo> _poses;
 
     // no copies
     AnimDebugDraw(const AnimDebugDraw&) = delete;