23 #include "skel_drawer.h"
25 #include <utils/math/angle.h>
27 #include <plugins/openni/utils/colors.h>
34 using namespace fawkes;
35 using namespace fawkes::openni;
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;