Fawkes API
Fawkes Development Version
|
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 }