23 #include "skel_drawer.h" 25 #include <utils/math/angle.h> 27 #include <plugins/openni/utils/colors.h> 48 : __users(users), __hands(hands)
54 SkelGuiSkeletonDrawer::print_string(
void *font,
char *str)
56 const int l = strlen(str);
57 for(
int i = 0; i < l; ++i) glutBitmapCharacter(font, *str++);
61 SkelGuiSkeletonDrawer::draw_limb(
float *proj1,
float conf1,
62 float *proj2,
float conf2)
64 if (conf1 < 0.5 || conf2 < 0.5)
return;
66 glVertex3i(proj1[0], proj1[1], 0);
67 glVertex3i(proj2[0], proj2[1], 0);
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()); 77 SkelGuiSkeletonDrawer::draw_user(
UserInfo &user)
79 if (user.
skel_if->
state() != HumanSkeletonInterface::STATE_TRACKING)
return;
81 DRAW_LIMB(user, head, neck);
83 DRAW_LIMB(user, neck, left_shoulder);
84 DRAW_LIMB(user, left_shoulder, left_elbow);
85 DRAW_LIMB(user, left_elbow, left_hand);
87 DRAW_LIMB(user, neck, right_shoulder);
88 DRAW_LIMB(user, right_shoulder, right_elbow);
89 DRAW_LIMB(user, right_elbow, right_hand);
91 DRAW_LIMB(user, left_shoulder, torso);
92 DRAW_LIMB(user, right_shoulder, torso);
94 DRAW_LIMB(user, torso, left_hip);
95 DRAW_LIMB(user, left_hip, left_knee);
96 DRAW_LIMB(user, left_knee, left_foot);
98 DRAW_LIMB(user, torso, right_hip);
99 DRAW_LIMB(user, right_hip, right_knee);
100 DRAW_LIMB(user, right_knee, right_foot);
102 DRAW_LIMB(user, left_hip, right_hip);
107 SkelGuiSkeletonDrawer::draw_circle(
unsigned int id,
float *proj,
float radius)
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],
115 for (
int i=0; i < 360; ++i) {
117 glVertex2f( proj[0] + cos(rad) * radius, proj[1] + sin(rad) * radius);
119 glColor4f(1, 1, 1, 1);
129 for (UserMap::iterator i = __users.begin(); i != __users.end(); ++i) {
130 if (i->second.skel_if->state() != HumanSkeletonInterface::STATE_INVALID) {
132 memset(label, 0,
sizeof(label));
134 sprintf(label,
"%s", i->first.c_str());
136 else if (i->second.skel_if->state() == HumanSkeletonInterface::STATE_TRACKING)
138 sprintf(label,
"%s - Tracking", i->first.c_str());
139 }
else if (i->second.skel_if->state() == HumanSkeletonInterface::STATE_CALIBRATING)
141 sprintf(label,
"%s - Calibrating...", i->first.c_str());
143 sprintf(label,
"%s - Looking for pose", i->first.c_str());
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],
151 glRasterPos2i(i->second.proj_if->proj_com(0), i->second.proj_if->proj_com(1));
152 print_string(GLUT_BITMAP_HELVETICA_18, label);
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],
161 draw_user(i->second);
166 glEnable(GL_LINE_SMOOTH);
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);
175 glDisable(GL_LINE_SMOOTH);
185 switch (__print_state) {
199 glBegin(GL_LINE_LOOP);
201 __print_state = state;
void toggle_print_state()
Toggle the printing state.
void draw()
Draw skeletons.
SkelGuiSkeletonDrawer(fawkes::openni::UserMap &users, fawkes::openni::HandMap &hands)
Constructor.
Fawkes library namespace.
State state() const
Get state value.
Print neither ID nor state.
fawkes::HumanSkeletonInterface * skel_if
Skeleton interface.
void set_print_state(PrintState state)
Set print state.
PrintState
Print state enum.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
User info to pass to draw_skeletons().