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 <GL/glut.h>
26#include <plugins/openni/utils/colors.h>
27#include <utils/math/angle.h>
28
29#include <cstdio>
30#include <cstring>
31
32using namespace fawkes;
33using namespace fawkes::openni;
34
35/** @class SkelGuiSkeletonDrawer "skel_drawer.h"
36 * Draw body skeleton using OpenGL.
37 * This class draws the limbs as read from the user interfaces.
38 * @author Tim Niemueller
39 */
40
41/** Constructor.
42 * @param users map of users shared with interface observer
43 * @param hands map of hands shared with interface observer
44 */
45SkelGuiSkeletonDrawer::SkelGuiSkeletonDrawer(UserMap &users, HandMap &hands)
46: users_(users), hands_(hands)
47{
48 print_state_ = PRINT_ID_STATE;
49}
50
51void
52SkelGuiSkeletonDrawer::print_string(void *font, char *str)
53{
54 const int l = strlen(str);
55 for (int i = 0; i < l; ++i)
56 glutBitmapCharacter(font, *str++);
57}
58
59void
60SkelGuiSkeletonDrawer::draw_limb(float *proj1, float conf1, float *proj2, float conf2)
61{
62 if (conf1 < 0.5 || conf2 < 0.5)
63 return;
64
65 glVertex3i(proj1[0], proj1[1], 0);
66 glVertex3i(proj2[0], proj2[1], 0);
67}
68
69#define DRAW_LIMB(user, joint1, joint2) \
70 draw_limb(user.proj_if->proj_##joint1(), \
71 user.skel_if->pos_##joint1##_confidence(), \
72 user.proj_if->proj_##joint2(), \
73 user.skel_if->pos_##joint2##_confidence());
74
75void
76SkelGuiSkeletonDrawer::draw_user(UserInfo &user)
77{
78 if (user.skel_if->state() != HumanSkeletonInterface::STATE_TRACKING)
79 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
105void
106SkelGuiSkeletonDrawer::draw_circle(unsigned int id, float *proj, float radius)
107{
108 glBegin(GL_LINE_LOOP);
109 glVertex2f(proj[0], proj[1]);
110 glColor4f(1 - USER_COLORS[id % NUM_USER_COLORS][0],
111 1 - USER_COLORS[id % NUM_USER_COLORS][1],
112 1 - USER_COLORS[id % NUM_USER_COLORS][2],
113 1);
114 for (int i = 0; i < 360; ++i) {
115 float rad = deg2rad(i);
116 ;
117 glVertex2f(proj[0] + cos(rad) * radius, proj[1] + sin(rad) * radius);
118 }
119 glColor4f(1, 1, 1, 1);
120 glEnd();
121}
122
123/** Draw skeletons. */
124void
126{
127 char label[50] = "";
128 for (UserMap::iterator i = users_.begin(); i != users_.end(); ++i) {
129 if (i->second.skel_if->state() != HumanSkeletonInterface::STATE_INVALID) {
130 if (print_state_ != PRINT_NONE) {
131 memset(label, 0, sizeof(label));
132 if (print_state_ == PRINT_ID) {
133 sprintf(label, "%s", i->first.c_str());
134 } else if (i->second.skel_if->state() == HumanSkeletonInterface::STATE_TRACKING) {
135 sprintf(label, "%s - Tracking", i->first.c_str());
136 } else if (i->second.skel_if->state() == HumanSkeletonInterface::STATE_CALIBRATING) {
137 sprintf(label, "%s - Calibrating...", i->first.c_str());
138 } else {
139 sprintf(label, "%s - Looking for pose", i->first.c_str());
140 }
141
142 glColor4f(1 - USER_COLORS[i->second.skel_if->user_id() % NUM_USER_COLORS][0],
143 1 - USER_COLORS[i->second.skel_if->user_id() % NUM_USER_COLORS][1],
144 1 - USER_COLORS[i->second.skel_if->user_id() % NUM_USER_COLORS][2],
145 1);
146
147 glRasterPos2i(i->second.proj_if->proj_com(0), i->second.proj_if->proj_com(1));
148 print_string(GLUT_BITMAP_HELVETICA_18, label);
149 }
150
151 glBegin(GL_LINES);
152 glColor4f(1 - USER_COLORS[i->second.skel_if->user_id() % NUM_USER_COLORS][0],
153 1 - USER_COLORS[i->second.skel_if->user_id() % NUM_USER_COLORS][1],
154 1 - USER_COLORS[i->second.skel_if->user_id() % NUM_USER_COLORS][2],
155 1);
156
157 draw_user(i->second);
158 glEnd();
159 }
160 }
161
162 glEnable(GL_LINE_SMOOTH);
163 glLineWidth(4);
164 for (HandMap::iterator i = hands_.begin(); i != hands_.end(); ++i) {
165 if (i->second.hand_if->is_visible()) {
166 float proj[2] = {i->second.hand_if->world_x(), i->second.hand_if->world_y()};
167 draw_circle(i->second.hand_if->world_z(), proj, 10);
168 }
169 }
170 glLineWidth(1.);
171 glDisable(GL_LINE_SMOOTH);
172}
173
174/** Toggle the printing state.
175 * This toggles through the printing state in the order PRINT_NONE,
176 * PRINT_ID_STATE, and PRINT_ID.
177 */
178void
180{
181 switch (print_state_) {
182 case PRINT_NONE: print_state_ = PRINT_ID_STATE; break;
183 case PRINT_ID_STATE: print_state_ = PRINT_ID; break;
184 case PRINT_ID: print_state_ = PRINT_NONE; break;
185 }
186}
187
188/** Set print state.
189 * @param state new print state
190 */
191void
193{
194 glBegin(GL_LINE_LOOP);
195
196 print_state_ = state;
197 glEnd();
198}
PrintState
Print state enum.
Definition: skel_drawer.h:35
@ PRINT_ID_STATE
Print ID and state.
Definition: skel_drawer.h:38
@ PRINT_NONE
Print neither ID nor state.
Definition: skel_drawer.h:36
@ PRINT_ID
Print only ID.
Definition: skel_drawer.h:37
void draw()
Draw skeletons.
void set_print_state(PrintState state)
Set print state.
void toggle_print_state()
Toggle the printing state.
SkelGuiSkeletonDrawer(fawkes::openni::UserMap &users, fawkes::openni::HandMap &hands)
Constructor.
Definition: skel_drawer.cpp:45
State state() const
Get state value.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36
User info to pass to draw_skeletons().
Definition: types.h:38
fawkes::HumanSkeletonInterface * skel_if
Skeleton interface.
Definition: types.h:39