Fawkes API  Fawkes Development Version
skel_drawer.cpp
1 
2 /***************************************************************************
3  * skel_drawer.cpp - Skeleton Visualization GUI: skeleton drawer
4  *
5  * Created: Wed Mar 02 11:36:43 2011
6  * Copyright 2006-2011 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "skel_drawer.h"
24 
25 #include <utils/math/angle.h>
26 
27 #include <plugins/openni/utils/colors.h>
28 
29 
30 #include <cstring>
31 #include <cstdio>
32 #include <GL/glut.h>
33 
34 using namespace fawkes;
35 using namespace fawkes::openni;
36 
37 /** @class SkelGuiSkeletonDrawer "skel_drawer.h"
38  * Draw body skeleton using OpenGL.
39  * This class draws the limbs as read from the user interfaces.
40  * @author Tim Niemueller
41  */
42 
43 /** Constructor.
44  * @param users map of users shared with interface observer
45  * @param hands map of hands shared with interface observer
46  */
47 SkelGuiSkeletonDrawer::SkelGuiSkeletonDrawer(UserMap &users, HandMap &hands)
48  : __users(users), __hands(hands)
49 {
50  __print_state = PRINT_ID_STATE;
51 }
52 
53 void
54 SkelGuiSkeletonDrawer::print_string(void *font, char *str)
55 {
56  const int l = strlen(str);
57  for(int i = 0; i < l; ++i) glutBitmapCharacter(font, *str++);
58 }
59 
60 void
61 SkelGuiSkeletonDrawer::draw_limb(float *proj1, float conf1,
62  float *proj2, float conf2)
63 {
64  if (conf1 < 0.5 || conf2 < 0.5) return;
65 
66  glVertex3i(proj1[0], proj1[1], 0);
67  glVertex3i(proj2[0], proj2[1], 0);
68 }
69 
70 #define DRAW_LIMB(user, joint1, joint2) \
71  draw_limb(user.proj_if->proj_##joint1(), \
72  user.skel_if->pos_##joint1##_confidence(), \
73  user.proj_if->proj_##joint2(), \
74  user.skel_if->pos_##joint2##_confidence());
75 
76 void
77 SkelGuiSkeletonDrawer::draw_user(UserInfo &user)
78 {
79  if (user.skel_if->state() != HumanSkeletonInterface::STATE_TRACKING) return;
80 
81  DRAW_LIMB(user, head, neck);
82 
83  DRAW_LIMB(user, neck, left_shoulder);
84  DRAW_LIMB(user, left_shoulder, left_elbow);
85  DRAW_LIMB(user, left_elbow, left_hand);
86 
87  DRAW_LIMB(user, neck, right_shoulder);
88  DRAW_LIMB(user, right_shoulder, right_elbow);
89  DRAW_LIMB(user, right_elbow, right_hand);
90 
91  DRAW_LIMB(user, left_shoulder, torso);
92  DRAW_LIMB(user, right_shoulder, torso);
93 
94  DRAW_LIMB(user, torso, left_hip);
95  DRAW_LIMB(user, left_hip, left_knee);
96  DRAW_LIMB(user, left_knee, left_foot);
97 
98  DRAW_LIMB(user, torso, right_hip);
99  DRAW_LIMB(user, right_hip, right_knee);
100  DRAW_LIMB(user, right_knee, right_foot);
101 
102  DRAW_LIMB(user, left_hip, right_hip);
103 
104 }
105 
106 void
107 SkelGuiSkeletonDrawer::draw_circle(unsigned int id, float *proj, float radius)
108 {
109  glBegin(GL_LINE_LOOP);
110  glVertex2f(proj[0], proj[1]);
111  glColor4f(1 - USER_COLORS[id % NUM_USER_COLORS][0],
112  1 - USER_COLORS[id % NUM_USER_COLORS][1],
113  1 - USER_COLORS[id % NUM_USER_COLORS][2],
114  1);
115  for (int i=0; i < 360; ++i) {
116  float rad = deg2rad(i);;
117  glVertex2f( proj[0] + cos(rad) * radius, proj[1] + sin(rad) * radius);
118  }
119  glColor4f(1, 1, 1, 1);
120  glEnd();
121 }
122 
123 
124 /** Draw skeletons. */
125 void
127 {
128  char label[50] = "";
129  for (UserMap::iterator i = __users.begin(); i != __users.end(); ++i) {
130  if (i->second.skel_if->state() != HumanSkeletonInterface::STATE_INVALID) {
131  if (__print_state != PRINT_NONE) {
132  memset(label, 0, sizeof(label));
133  if (__print_state == PRINT_ID) {
134  sprintf(label, "%s", i->first.c_str());
135  }
136  else if (i->second.skel_if->state() == HumanSkeletonInterface::STATE_TRACKING)
137  {
138  sprintf(label, "%s - Tracking", i->first.c_str());
139  } else if (i->second.skel_if->state() == HumanSkeletonInterface::STATE_CALIBRATING)
140  {
141  sprintf(label, "%s - Calibrating...", i->first.c_str());
142  } else {
143  sprintf(label, "%s - Looking for pose", i->first.c_str());
144  }
145 
146  glColor4f(1 - USER_COLORS[i->second.skel_if->user_id() % NUM_USER_COLORS][0],
147  1 - USER_COLORS[i->second.skel_if->user_id() % NUM_USER_COLORS][1],
148  1 - USER_COLORS[i->second.skel_if->user_id() % NUM_USER_COLORS][2],
149  1);
150 
151  glRasterPos2i(i->second.proj_if->proj_com(0), i->second.proj_if->proj_com(1));
152  print_string(GLUT_BITMAP_HELVETICA_18, label);
153  }
154 
155  glBegin(GL_LINES);
156  glColor4f(1 - USER_COLORS[i->second.skel_if->user_id() % NUM_USER_COLORS][0],
157  1 - USER_COLORS[i->second.skel_if->user_id() % NUM_USER_COLORS][1],
158  1 - USER_COLORS[i->second.skel_if->user_id() % NUM_USER_COLORS][2],
159  1);
160 
161  draw_user(i->second);
162  glEnd();
163  }
164  }
165 
166  glEnable(GL_LINE_SMOOTH);
167  glLineWidth(4);
168  for (HandMap::iterator i = __hands.begin(); i != __hands.end(); ++i) {
169  if (i->second.hand_if->is_visible()) {
170  float proj[2] = {i->second.hand_if->world_x(), i->second.hand_if->world_y()};
171  draw_circle(i->second.hand_if->world_z(), proj, 10);
172  }
173  }
174  glLineWidth(1.);
175  glDisable(GL_LINE_SMOOTH);
176 }
177 
178 /** Toggle the printing state.
179  * This toggles through the printing state in the order PRINT_NONE,
180  * PRINT_ID_STATE, and PRINT_ID.
181  */
182 void
184 {
185  switch (__print_state) {
186  case PRINT_NONE: __print_state = PRINT_ID_STATE; break;
187  case PRINT_ID_STATE: __print_state = PRINT_ID; break;
188  case PRINT_ID: __print_state = PRINT_NONE; break;
189  }
190 }
191 
192 
193 /** Set print state.
194  * @param state new print state
195  */
196 void
198 {
199  glBegin(GL_LINE_LOOP);
200 
201  __print_state = state;
202  glEnd();
203 }
void toggle_print_state()
Toggle the printing state.
void draw()
Draw skeletons.
SkelGuiSkeletonDrawer(fawkes::openni::UserMap &users, fawkes::openni::HandMap &hands)
Constructor.
Definition: skel_drawer.cpp:47
Fawkes library namespace.
State state() const
Get state value.
Print neither ID nor state.
Definition: skel_drawer.h:37
fawkes::HumanSkeletonInterface * skel_if
Skeleton interface.
Definition: types.h:42
void set_print_state(PrintState state)
Set print state.
PrintState
Print state enum.
Definition: skel_drawer.h:36
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:37
User info to pass to draw_skeletons().
Definition: types.h:41