Fawkes API  Fawkes Development Version
skel_drawer.cpp
1 
2 /***************************************************************************
3  * skel_drawer.cpp - OpenNI Visualization: 3D skeleton drawer
4  *
5  * Created: Sat Apr 02 20:00:50 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 #include <plugins/openni/utils/colors.h>
25 
26 #include <utils/math/angle.h>
27 
28 #include <cstring>
29 #include <cstdio>
30 #include <GL/glut.h>
31 
32 using namespace fawkes;
33 using namespace fawkes::openni;
34 
35 /** @class SkelGuiSkeletonDrawer3D "skel_drawer.h"
36  * Draw body skeleton using OpenGL (3D).
37  * This class draws the limbs as read from the user interfaces. This version
38  * draws in 3D and does not use the 2D projection.
39  * @author Tim Niemueller
40  */
41 
42 /** Constructor.
43  * @param users map of users shared with interface observer
44  * @param hands map of hands shared with interface observer
45  */
46 SkelGuiSkeletonDrawer3D::SkelGuiSkeletonDrawer3D(UserMap &users, HandMap &hands)
47  : __users(users), __hands(hands)
48 {
49  __print_state = PRINT_ID_STATE;
50 }
51 
52 void
53 SkelGuiSkeletonDrawer3D::draw_limb(float *p1, float conf1,
54  float *p2, float conf2)
55 {
56  if (conf1 < 0.5 || conf2 < 0.5) return;
57 
58  //printf("Drawing from (%f,%f,%f) -> (%f,%f,%f)\n",
59  // p1[0], p1[1], p1[2], p2[0], p2[1], p2[2]);
60  glVertex4f(p1[0], p1[1], p1[2], 1);
61  glVertex4f(p2[0], p2[1], p2[2], 1);
62 }
63 
64 #define DRAW_LIMB(user, joint1, joint2) \
65  draw_limb(user.skel_if->pos_##joint1(), \
66  user.skel_if->pos_##joint1##_confidence(), \
67  user.skel_if->pos_##joint2(), \
68  user.skel_if->pos_##joint2##_confidence());
69 
70 void
71 SkelGuiSkeletonDrawer3D::draw_user(UserInfo &user)
72 {
73  if (user.skel_if->state() != HumanSkeletonInterface::STATE_TRACKING) return;
74 
75  DRAW_LIMB(user, head, neck);
76 
77  DRAW_LIMB(user, neck, left_shoulder);
78  DRAW_LIMB(user, left_shoulder, left_elbow);
79  DRAW_LIMB(user, left_elbow, left_hand);
80 
81  DRAW_LIMB(user, neck, right_shoulder);
82  DRAW_LIMB(user, right_shoulder, right_elbow);
83  DRAW_LIMB(user, right_elbow, right_hand);
84 
85  DRAW_LIMB(user, left_shoulder, torso);
86  DRAW_LIMB(user, right_shoulder, torso);
87 
88  DRAW_LIMB(user, torso, left_hip);
89  DRAW_LIMB(user, left_hip, left_knee);
90  DRAW_LIMB(user, left_knee, left_foot);
91 
92  DRAW_LIMB(user, torso, right_hip);
93  DRAW_LIMB(user, right_hip, right_knee);
94  DRAW_LIMB(user, right_knee, right_foot);
95 
96  DRAW_LIMB(user, left_hip, right_hip);
97 
98 }
99 
100 /** Draw skeletons. */
101 void
103 {
104  for (UserMap::iterator i = __users.begin(); i != __users.end(); ++i) {
105  i->second.skel_if->read();
106  if (i->second.skel_if->state() != HumanSkeletonInterface::STATE_INVALID) {
107  glPointSize(10);
108  glBegin(GL_POINTS);
109  glColor4f(1 - USER_COLORS[i->second.skel_if->user_id() % NUM_USER_COLORS][0],
110  1 - USER_COLORS[i->second.skel_if->user_id() % NUM_USER_COLORS][1],
111  1 - USER_COLORS[i->second.skel_if->user_id() % NUM_USER_COLORS][2],
112  1);
113  float *com = i->second.skel_if->com();
114  glVertex4f(com[0], com[1], com[2], 1.0);
115  glEnd();
116  glPointSize(1);
117 
118  glLineWidth(3);
119  glBegin(GL_LINES);
120  draw_user(i->second);
121  glColor4f(1, 1, 1, 1);
122  glEnd();
123  glLineWidth(1);
124  }
125  }
126 
127  /*
128  glEnable(GL_LINE_SMOOTH);
129  glLineWidth(4);
130  for (HandMap::iterator i = __hands.begin(); i != __hands.end(); ++i) {
131  if (i->second.hand_if->is_visible()) {
132  float proj[2] = {i->second.hand_if->world_x(), i->second.hand_if->world_y()};
133  draw_circle(i->second.hand_if->world_z(), proj, 10);
134  }
135  }
136  glLineWidth(1.);
137  glDisable(GL_LINE_SMOOTH);
138  */
139 }
140 
141 /** Toggle the printing state.
142  * This toggles through the printing state in the order PRINT_NONE,
143  * PRINT_ID_STATE, and PRINT_ID.
144  */
145 void
147 {
148  switch (__print_state) {
149  case PRINT_NONE: __print_state = PRINT_ID_STATE; break;
150  case PRINT_ID_STATE: __print_state = PRINT_ID; break;
151  case PRINT_ID: __print_state = PRINT_NONE; break;
152  }
153 }
154 
155 
156 void
157 SkelGuiSkeletonDrawer3D::draw_circle(unsigned int id, float *p, float radius)
158 {
159  glBegin(GL_LINE_LOOP);
160  glVertex3f(p[0], p[1], p[2]);
161  glColor4f(1 - USER_COLORS[id % NUM_USER_COLORS][0],
162  1 - USER_COLORS[id % NUM_USER_COLORS][1],
163  1 - USER_COLORS[id % NUM_USER_COLORS][2],
164  1);
165  for (int i=0; i < 360; ++i) {
166  float rad = deg2rad(i);;
167  glVertex3f( p[0] + cos(rad) * radius, p[1] + sin(rad) * radius, p[2]);
168  }
169  glColor4f(1, 1, 1, 1);
170  glEnd();
171 }
172 
173 
174 /** Set print state.
175  * @param state new print state
176  */
177 void
179 {
180  __print_state = state;
181 }
Fawkes library namespace.
Print neither ID nor state.
Definition: skel_drawer.h:37
void draw()
Draw skeletons.
void set_print_state(PrintState state)
Set print state.
PrintState
Print state enum.
Definition: skel_drawer.h:36
State state() const
Get state value.
fawkes::HumanSkeletonInterface * skel_if
Skeleton interface.
Definition: types.h:42
SkelGuiSkeletonDrawer3D(fawkes::openni::UserMap &users, fawkes::openni::HandMap &hands)
Constructor.
Definition: skel_drawer.cpp:46
void toggle_print_state()
Toggle the printing state.
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