Fawkes API  Fawkes Development Version
skel_drawer.cpp
00001 
00002 /***************************************************************************
00003  *  skel_drawer.cpp - Skeleton Visualization GUI: skeleton drawer
00004  *
00005  *  Created: Wed Mar 02 11:36:43 2011
00006  *  Copyright  2006-2011  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #include "skel_drawer.h"
00024 
00025 #include <utils/math/angle.h>
00026 
00027 #include <plugins/openni/utils/colors.h>
00028 
00029 
00030 #include <cstring>
00031 #include <cstdio>
00032 #include <GL/glut.h>
00033 
00034 using namespace fawkes;
00035 using namespace fawkes::openni;
00036 
00037 /** @class SkelGuiSkeletonDrawer "skel_drawer.h"
00038  * Draw body skeleton using OpenGL.
00039  * This class draws the limbs as read from the user interfaces.
00040  * @author Tim Niemueller
00041  */
00042 
00043 /** Constructor.
00044  * @param users map of users shared with interface observer
00045  * @param hands map of hands shared with interface observer
00046  */
00047 SkelGuiSkeletonDrawer::SkelGuiSkeletonDrawer(UserMap &users, HandMap &hands)
00048   : __users(users), __hands(hands)
00049 {
00050   __print_state = PRINT_ID_STATE;
00051 }
00052 
00053 void
00054 SkelGuiSkeletonDrawer::print_string(void *font, char *str)
00055 {
00056   const int l = strlen(str);
00057   for(int i = 0; i < l; ++i)  glutBitmapCharacter(font, *str++);
00058 }
00059 
00060 void
00061 SkelGuiSkeletonDrawer::draw_limb(float *proj1, float conf1,
00062                                  float *proj2, float conf2)
00063 {
00064   if (conf1 < 0.5 || conf2 < 0.5)  return;
00065 
00066   glVertex3i(proj1[0], proj1[1], 0);
00067   glVertex3i(proj2[0], proj2[1], 0);
00068 }
00069 
00070 #define DRAW_LIMB(user, joint1, joint2)                                 \
00071   draw_limb(user.proj_if->proj_##joint1(),                              \
00072             user.skel_if->pos_##joint1##_confidence(),                  \
00073             user.proj_if->proj_##joint2(),                              \
00074             user.skel_if->pos_##joint2##_confidence());
00075 
00076 void
00077 SkelGuiSkeletonDrawer::draw_user(UserInfo &user)
00078 {
00079   if (user.skel_if->state() != HumanSkeletonInterface::STATE_TRACKING)  return;
00080 
00081   DRAW_LIMB(user, head, neck);
00082   
00083   DRAW_LIMB(user, neck, left_shoulder);
00084   DRAW_LIMB(user, left_shoulder, left_elbow);
00085   DRAW_LIMB(user, left_elbow, left_hand);
00086   
00087   DRAW_LIMB(user, neck, right_shoulder);
00088   DRAW_LIMB(user, right_shoulder, right_elbow);
00089   DRAW_LIMB(user, right_elbow, right_hand);
00090 
00091   DRAW_LIMB(user, left_shoulder, torso);
00092   DRAW_LIMB(user, right_shoulder, torso);
00093 
00094   DRAW_LIMB(user, torso, left_hip);
00095   DRAW_LIMB(user, left_hip, left_knee);
00096   DRAW_LIMB(user, left_knee, left_foot);
00097 
00098   DRAW_LIMB(user, torso, right_hip);
00099   DRAW_LIMB(user, right_hip, right_knee);
00100   DRAW_LIMB(user, right_knee, right_foot);
00101 
00102   DRAW_LIMB(user, left_hip, right_hip);
00103 
00104 }
00105 
00106 void
00107 SkelGuiSkeletonDrawer::draw_circle(unsigned int id, float *proj, float radius)
00108 {
00109   glBegin(GL_LINE_LOOP);
00110   glVertex2f(proj[0], proj[1]);
00111   glColor4f(1 - USER_COLORS[id % NUM_USER_COLORS][0],
00112             1 - USER_COLORS[id % NUM_USER_COLORS][1],
00113             1 - USER_COLORS[id % NUM_USER_COLORS][2],
00114             1);
00115   for (int i=0; i < 360; ++i) {
00116     float rad = deg2rad(i);;
00117     glVertex2f( proj[0] + cos(rad) * radius, proj[1] + sin(rad) * radius);
00118   }
00119   glColor4f(1, 1, 1, 1);
00120   glEnd();
00121 }
00122 
00123 
00124 /** Draw skeletons. */
00125 void
00126 SkelGuiSkeletonDrawer::draw()
00127 {
00128   char label[50] = "";
00129   for (UserMap::iterator i = __users.begin(); i != __users.end(); ++i) {
00130     if (i->second.skel_if->state() != HumanSkeletonInterface::STATE_INVALID) {
00131       if (__print_state != PRINT_NONE) {
00132         memset(label, 0, sizeof(label));
00133         if (__print_state == PRINT_ID) {
00134           sprintf(label, "%s", i->first.c_str());
00135         }
00136         else if (i->second.skel_if->state() == HumanSkeletonInterface::STATE_TRACKING)
00137         {
00138           sprintf(label, "%s - Tracking", i->first.c_str());
00139         } else if (i->second.skel_if->state() == HumanSkeletonInterface::STATE_CALIBRATING)
00140         {
00141           sprintf(label, "%s - Calibrating...", i->first.c_str());
00142         } else {
00143         sprintf(label, "%s - Looking for pose", i->first.c_str());
00144         }
00145 
00146         glColor4f(1 - USER_COLORS[i->second.skel_if->user_id() % NUM_USER_COLORS][0],
00147                   1 - USER_COLORS[i->second.skel_if->user_id() % NUM_USER_COLORS][1],
00148                   1 - USER_COLORS[i->second.skel_if->user_id() % NUM_USER_COLORS][2],
00149                   1);
00150       
00151         glRasterPos2i(i->second.proj_if->proj_com(0), i->second.proj_if->proj_com(1));
00152         print_string(GLUT_BITMAP_HELVETICA_18, label);
00153       }
00154 
00155       glBegin(GL_LINES);
00156       glColor4f(1 - USER_COLORS[i->second.skel_if->user_id() % NUM_USER_COLORS][0],
00157                 1 - USER_COLORS[i->second.skel_if->user_id() % NUM_USER_COLORS][1],
00158                 1 - USER_COLORS[i->second.skel_if->user_id() % NUM_USER_COLORS][2],
00159                 1);
00160 
00161       draw_user(i->second);
00162       glEnd();
00163     }
00164   }
00165 
00166   glEnable(GL_LINE_SMOOTH);
00167   glLineWidth(4);
00168   for (HandMap::iterator i = __hands.begin(); i != __hands.end(); ++i) {
00169     if (i->second.hand_if->is_visible()) {
00170       float proj[2] = {i->second.hand_if->world_x(), i->second.hand_if->world_y()};
00171       draw_circle(i->second.hand_if->world_z(), proj, 10);
00172     }
00173   }
00174   glLineWidth(1.);
00175   glDisable(GL_LINE_SMOOTH);
00176 }
00177 
00178 /** Toggle the printing state.
00179  * This toggles through the printing state in the order PRINT_NONE,
00180  * PRINT_ID_STATE, and PRINT_ID.
00181  */
00182 void
00183 SkelGuiSkeletonDrawer::toggle_print_state()
00184 {
00185   switch (__print_state) {
00186   case PRINT_NONE:      __print_state = PRINT_ID_STATE; break;
00187   case PRINT_ID_STATE:  __print_state = PRINT_ID;       break;
00188   case PRINT_ID:        __print_state = PRINT_NONE;     break;
00189   }
00190 }
00191 
00192 
00193 /** Set print state.
00194  * @param state new print state
00195  */
00196 void
00197 SkelGuiSkeletonDrawer::set_print_state(SkelGuiSkeletonDrawer::PrintState state)
00198 {
00199   glBegin(GL_LINE_LOOP);
00200 
00201   __print_state = state;
00202   glEnd();
00203 }