Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * skel_drawer.cpp - OpenNI Visualization: 3D skeleton drawer 00004 * 00005 * Created: Sat Apr 02 20:00:50 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 #include <plugins/openni/utils/colors.h> 00025 00026 #include <utils/math/angle.h> 00027 00028 #include <cstring> 00029 #include <cstdio> 00030 #include <GL/glut.h> 00031 00032 using namespace fawkes; 00033 using namespace fawkes::openni; 00034 00035 /** @class SkelGuiSkeletonDrawer3D "skel_drawer.h" 00036 * Draw body skeleton using OpenGL (3D). 00037 * This class draws the limbs as read from the user interfaces. This version 00038 * draws in 3D and does not use the 2D projection. 00039 * @author Tim Niemueller 00040 */ 00041 00042 /** Constructor. 00043 * @param users map of users shared with interface observer 00044 * @param hands map of hands shared with interface observer 00045 */ 00046 SkelGuiSkeletonDrawer3D::SkelGuiSkeletonDrawer3D(UserMap &users, HandMap &hands) 00047 : __users(users), __hands(hands) 00048 { 00049 __print_state = PRINT_ID_STATE; 00050 } 00051 00052 void 00053 SkelGuiSkeletonDrawer3D::draw_limb(float *p1, float conf1, 00054 float *p2, float conf2) 00055 { 00056 if (conf1 < 0.5 || conf2 < 0.5) return; 00057 00058 //printf("Drawing from (%f,%f,%f) -> (%f,%f,%f)\n", 00059 // p1[0], p1[1], p1[2], p2[0], p2[1], p2[2]); 00060 glVertex4f(p1[0], p1[1], p1[2], 1); 00061 glVertex4f(p2[0], p2[1], p2[2], 1); 00062 } 00063 00064 #define DRAW_LIMB(user, joint1, joint2) \ 00065 draw_limb(user.skel_if->pos_##joint1(), \ 00066 user.skel_if->pos_##joint1##_confidence(), \ 00067 user.skel_if->pos_##joint2(), \ 00068 user.skel_if->pos_##joint2##_confidence()); 00069 00070 void 00071 SkelGuiSkeletonDrawer3D::draw_user(UserInfo &user) 00072 { 00073 if (user.skel_if->state() != HumanSkeletonInterface::STATE_TRACKING) return; 00074 00075 DRAW_LIMB(user, head, neck); 00076 00077 DRAW_LIMB(user, neck, left_shoulder); 00078 DRAW_LIMB(user, left_shoulder, left_elbow); 00079 DRAW_LIMB(user, left_elbow, left_hand); 00080 00081 DRAW_LIMB(user, neck, right_shoulder); 00082 DRAW_LIMB(user, right_shoulder, right_elbow); 00083 DRAW_LIMB(user, right_elbow, right_hand); 00084 00085 DRAW_LIMB(user, left_shoulder, torso); 00086 DRAW_LIMB(user, right_shoulder, torso); 00087 00088 DRAW_LIMB(user, torso, left_hip); 00089 DRAW_LIMB(user, left_hip, left_knee); 00090 DRAW_LIMB(user, left_knee, left_foot); 00091 00092 DRAW_LIMB(user, torso, right_hip); 00093 DRAW_LIMB(user, right_hip, right_knee); 00094 DRAW_LIMB(user, right_knee, right_foot); 00095 00096 DRAW_LIMB(user, left_hip, right_hip); 00097 00098 } 00099 00100 /** Draw skeletons. */ 00101 void 00102 SkelGuiSkeletonDrawer3D::draw() 00103 { 00104 for (UserMap::iterator i = __users.begin(); i != __users.end(); ++i) { 00105 i->second.skel_if->read(); 00106 if (i->second.skel_if->state() != HumanSkeletonInterface::STATE_INVALID) { 00107 glPointSize(10); 00108 glBegin(GL_POINTS); 00109 glColor4f(1 - USER_COLORS[i->second.skel_if->user_id() % NUM_USER_COLORS][0], 00110 1 - USER_COLORS[i->second.skel_if->user_id() % NUM_USER_COLORS][1], 00111 1 - USER_COLORS[i->second.skel_if->user_id() % NUM_USER_COLORS][2], 00112 1); 00113 float *com = i->second.skel_if->com(); 00114 glVertex4f(com[0], com[1], com[2], 1.0); 00115 glEnd(); 00116 glPointSize(1); 00117 00118 glLineWidth(3); 00119 glBegin(GL_LINES); 00120 draw_user(i->second); 00121 glColor4f(1, 1, 1, 1); 00122 glEnd(); 00123 glLineWidth(1); 00124 } 00125 } 00126 00127 /* 00128 glEnable(GL_LINE_SMOOTH); 00129 glLineWidth(4); 00130 for (HandMap::iterator i = __hands.begin(); i != __hands.end(); ++i) { 00131 if (i->second.hand_if->is_visible()) { 00132 float proj[2] = {i->second.hand_if->world_x(), i->second.hand_if->world_y()}; 00133 draw_circle(i->second.hand_if->world_z(), proj, 10); 00134 } 00135 } 00136 glLineWidth(1.); 00137 glDisable(GL_LINE_SMOOTH); 00138 */ 00139 } 00140 00141 /** Toggle the printing state. 00142 * This toggles through the printing state in the order PRINT_NONE, 00143 * PRINT_ID_STATE, and PRINT_ID. 00144 */ 00145 void 00146 SkelGuiSkeletonDrawer3D::toggle_print_state() 00147 { 00148 switch (__print_state) { 00149 case PRINT_NONE: __print_state = PRINT_ID_STATE; break; 00150 case PRINT_ID_STATE: __print_state = PRINT_ID; break; 00151 case PRINT_ID: __print_state = PRINT_NONE; break; 00152 } 00153 } 00154 00155 00156 void 00157 SkelGuiSkeletonDrawer3D::draw_circle(unsigned int id, float *p, float radius) 00158 { 00159 glBegin(GL_LINE_LOOP); 00160 glVertex3f(p[0], p[1], p[2]); 00161 glColor4f(1 - USER_COLORS[id % NUM_USER_COLORS][0], 00162 1 - USER_COLORS[id % NUM_USER_COLORS][1], 00163 1 - USER_COLORS[id % NUM_USER_COLORS][2], 00164 1); 00165 for (int i=0; i < 360; ++i) { 00166 float rad = deg2rad(i);; 00167 glVertex3f( p[0] + cos(rad) * radius, p[1] + sin(rad) * radius, p[2]); 00168 } 00169 glColor4f(1, 1, 1, 1); 00170 glEnd(); 00171 } 00172 00173 00174 /** Set print state. 00175 * @param state new print state 00176 */ 00177 void 00178 SkelGuiSkeletonDrawer3D::set_print_state(SkelGuiSkeletonDrawer3D::PrintState state) 00179 { 00180 __print_state = state; 00181 }