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
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 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 */
47: users_(users), hands_(hands)
48{
49 print_state_ = PRINT_ID_STATE;
50}
51
52void
53SkelGuiSkeletonDrawer3D::draw_limb(float *p1, float conf1, float *p2, float conf2)
54{
55 if (conf1 < 0.5 || conf2 < 0.5)
56 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
70void
71SkelGuiSkeletonDrawer3D::draw_user(UserInfo &user)
72{
73 if (user.skel_if->state() != HumanSkeletonInterface::STATE_TRACKING)
74 return;
75
76 DRAW_LIMB(user, head, neck);
77
78 DRAW_LIMB(user, neck, left_shoulder);
79 DRAW_LIMB(user, left_shoulder, left_elbow);
80 DRAW_LIMB(user, left_elbow, left_hand);
81
82 DRAW_LIMB(user, neck, right_shoulder);
83 DRAW_LIMB(user, right_shoulder, right_elbow);
84 DRAW_LIMB(user, right_elbow, right_hand);
85
86 DRAW_LIMB(user, left_shoulder, torso);
87 DRAW_LIMB(user, right_shoulder, torso);
88
89 DRAW_LIMB(user, torso, left_hip);
90 DRAW_LIMB(user, left_hip, left_knee);
91 DRAW_LIMB(user, left_knee, left_foot);
92
93 DRAW_LIMB(user, torso, right_hip);
94 DRAW_LIMB(user, right_hip, right_knee);
95 DRAW_LIMB(user, right_knee, right_foot);
96
97 DRAW_LIMB(user, left_hip, right_hip);
98}
99
100/** Draw skeletons. */
101void
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 */
145void
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
155void
156SkelGuiSkeletonDrawer3D::draw_circle(unsigned int id, float *p, float radius)
157{
158 glBegin(GL_LINE_LOOP);
159 glVertex3f(p[0], p[1], p[2]);
160 glColor4f(1 - USER_COLORS[id % NUM_USER_COLORS][0],
161 1 - USER_COLORS[id % NUM_USER_COLORS][1],
162 1 - USER_COLORS[id % NUM_USER_COLORS][2],
163 1);
164 for (int i = 0; i < 360; ++i) {
165 float rad = deg2rad(i);
166 ;
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/** Set print state.
174 * @param state new print state
175 */
176void
178{
179 print_state_ = state;
180}
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.
SkelGuiSkeletonDrawer3D(fawkes::openni::UserMap &users, fawkes::openni::HandMap &hands)
Constructor.
Definition: skel_drawer.cpp:46
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