Skeleton Visualizer

SkeletonVisualizer.cpp

#include "SkeletonVisualizer.h"
#if __linux
    #include <GL/glew.h>
    #include <GL/gl.h>
#elif WIN32
    #include <GL/glew.h>
    #include <Windows.h>
    #include <tchar.h>
    #include <gl/gl.h>
#else
    #include <OpenGL/gl.h>
#endif

// API: void DrawBone(const glm::mat4& bone, float scale = 1.0f);
//      void DrawBone(const glm::mat4& parent, const glm::mat4& bone, float scale 1.0f);
//      void DrawLines();
//      void DrawOctahedron();


namespace SkeletonVisualizerPrivate {
    static glm::vec3 renderColors[] = {
        {1.0f, 0.0f, 0.0f},
        {0.0f, 1.0f, 0.0f},
        {0.0f, 0.0f, 1.0f},
        {1.0f, 1.0f, 0.0f},
        {0.0f, 1.0f, 1.0f},
        {1.0f, 0.0f, 1.0f},
    };
    static int currentRenderIndex = 0;
    static int maxRenderIndex = 6;
    static bool useLines = false;
    
    // From: http://ozark.hendrix.edu/~burch/cs/490/sched/feb8/
    void DrawSphere() {
        int i, j;
        int lats = 4; // 10
        int longs = 4; // 10
        for(i = 0; i <= lats; i++) {
            double lat0 = M_PI * (-0.5 + (double) (i - 1) / lats);
            double z0  = sin(lat0);
            double zr0 =  cos(lat0);

            double lat1 = M_PI * (-0.5 + (double) i / lats);
            double z1 = sin(lat1);
            double zr1 = cos(lat1);

            glBegin(GL_QUAD_STRIP);
            for(j = 0; j <= longs; j++) {
                double lng = 2 * M_PI * (double) (j - 1) / longs;
                double x = cos(lng);
                double y = sin(lng);

                glNormal3f(x * zr0, y * zr0, z0);
                glVertex3f(x * zr0, y * zr0, z0);
                glNormal3f(x * zr1, y * zr1, z1);
                glVertex3f(x * zr1, y * zr1, z1);
            }
            glEnd();
        }
    }
};
using namespace SkeletonVisualizerPrivate;

void DrawLines() {
    useLines = true;
    currentRenderIndex = 0;
}

void DrawOctahedron() {
    useLines = false;
    currentRenderIndex = 0;
}

void DrawBone(const glm::mat4& bone, float scale) {
    glMatrixMode(GL_MODELVIEW);
    glPushMatrix();
    
    glDisable(GL_DEPTH_TEST);
    glDisable(GL_TEXTURE_2D);
    glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
    
    glColor3f(renderColors[currentRenderIndex].x, renderColors[currentRenderIndex].y, renderColors[currentRenderIndex].z);
    currentRenderIndex = currentRenderIndex + 1 >= maxRenderIndex ? 0 : currentRenderIndex + 1;
    glTranslatef(bone[3][0], bone[3][1], bone[3][2]);
    glScalef(scale, scale, scale);
    DrawSphere();

    glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_TEXTURE_2D);
    
    glPopMatrix();
}

void DrawBone(const glm::mat4& parent, const glm::mat4& bone, float scale) {
    glDisable(GL_DEPTH_TEST);
    glDisable(GL_TEXTURE_2D);
    glDisable(GL_CULL_FACE);
    glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
    glMatrixMode(GL_MODELVIEW);
    glColor3f(renderColors[currentRenderIndex].x, renderColors[currentRenderIndex].y, renderColors[currentRenderIndex].z);
    currentRenderIndex = currentRenderIndex + 1 >= maxRenderIndex ? 0 : currentRenderIndex + 1;
    
    if (useLines) {
        glBegin(GL_LINES);
        glVertex3f(bone[3][0], bone[3][1], bone[3][2]);
        glVertex3f(parent[3][0], parent[3][1], parent[3][2]);
        glEnd();
    }
    else {
        // Draw a sphere for the bone
        glPushMatrix();
        glTranslatef(bone[3][0], bone[3][1], bone[3][2]);
        glScalef(scale, scale, scale);
        DrawSphere();
        glPopMatrix();
        
        // Position, and directions of bone matrix
        glm::vec3 position = glm::vec3(bone[3][0], bone[3][1], bone[3][2]);
        glm::vec3 forward = glm::normalize(glm::vec3(bone[0][2], bone[1][2], bone[2][2]));
        glm::vec3 up = glm::normalize(glm::cross(glm::vec3(bone[0][0], bone[1][0], bone[2][0]), forward));
        //glm::vec3 right = glm::normalize(glm::cross(up, forward));
        
        // Quarter directions
        glm::vec3 quarterForward = ((glm::vec3(parent[3][0], parent[3][1], parent[3][2]) - position) * 0.25f);
        glm::vec3 quarterRight = glm::normalize(glm::cross(quarterForward, up)) * scale;
        glm::vec3 quarterUp = glm::normalize(glm::cross(quarterForward, quarterRight)) * scale;
        glm::vec3 quarterPoint = position + quarterForward;
        
        // Edges
        glm::vec3 p1 = glm::vec3(quarterPoint.x + quarterRight.x, quarterPoint.y + quarterRight.y, quarterPoint.z + quarterRight.z);
        glm::vec3 p2 = glm::vec3(quarterPoint.x - quarterRight.x, quarterPoint.y - quarterRight.y, quarterPoint.z - quarterRight.z);
        glm::vec3 p3 = glm::vec3(quarterPoint.x + quarterUp.x, quarterPoint.y + quarterUp.y, quarterPoint.z + quarterUp.z);
        glm::vec3 p4 = glm::vec3(quarterPoint.x - quarterUp.x, quarterPoint.y - quarterUp.y, quarterPoint.z - quarterUp.z);

        
        glBegin(GL_LINES);
        // Joint to square
        glVertex3f(p1.x, p1.y, p1.z);
        glVertex3f(position.x, position.y, position.z);
        glVertex3f(p2.x, p2.y, p2.z);
        glVertex3f(position.x, position.y, position.z);
        glVertex3f(p3.x, p3.y, p3.z);
        glVertex3f(position.x, position.y, position.z);
        glVertex3f(p4.x, p4.y, p4.z);
        glVertex3f(position.x, position.y, position.z);
        
        // parent to square
        glVertex3f(p1.x, p1.y, p1.z);
        glVertex3f(parent[3][0], parent[3][1], parent[3][2]);
        glVertex3f(p2.x, p2.y, p2.z);
        glVertex3f(parent[3][0], parent[3][1], parent[3][2]);
        glVertex3f(p3.x, p3.y, p3.z);
        glVertex3f(parent[3][0], parent[3][1], parent[3][2]);
        glVertex3f(p4.x, p4.y, p4.z);
        glVertex3f(parent[3][0], parent[3][1], parent[3][2]);
        
        // Draw Square
        glVertex3f(p1.x, p1.y, p1.z);
        glVertex3f(p3.x, p3.y, p3.z);
        glVertex3f(p3.x, p3.y, p3.z);
        glVertex3f(p2.x, p2.y, p2.z);
        glVertex3f(p2.x, p2.y, p2.z);
        glVertex3f(p4.x, p4.y, p4.z);
        glVertex3f(p4.x, p4.y, p4.z);
        glVertex3f(p1.x, p1.y, p1.z);

        glEnd();
    }
    
    glEnable(GL_CULL_FACE);
    glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_TEXTURE_2D);
}