23 #include "skel_drawer.h"
26 #include <plugins/openni/utils/colors.h>
27 #include <utils/math/angle.h>
33 using namespace fawkes::openni;
47 : users_(users), hands_(hands)
53 SkelGuiSkeletonDrawer3D::draw_limb(
float *p1,
float conf1,
float *p2,
float conf2)
55 if (conf1 < 0.5 || conf2 < 0.5)
60 glVertex4f(p1[0], p1[1], p1[2], 1);
61 glVertex4f(p2[0], p2[1], p2[2], 1);
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());
71 SkelGuiSkeletonDrawer3D::draw_user(
UserInfo &user)
73 if (user.
skel_if->
state() != HumanSkeletonInterface::STATE_TRACKING)
76 DRAW_LIMB(user, head, neck);
78 DRAW_LIMB(user, neck, left_shoulder);
79 DRAW_LIMB(user, left_shoulder, left_elbow);
80 DRAW_LIMB(user, left_elbow, left_hand);
82 DRAW_LIMB(user, neck, right_shoulder);
83 DRAW_LIMB(user, right_shoulder, right_elbow);
84 DRAW_LIMB(user, right_elbow, right_hand);
86 DRAW_LIMB(user, left_shoulder, torso);
87 DRAW_LIMB(user, right_shoulder, torso);
89 DRAW_LIMB(user, torso, left_hip);
90 DRAW_LIMB(user, left_hip, left_knee);
91 DRAW_LIMB(user, left_knee, left_foot);
93 DRAW_LIMB(user, torso, right_hip);
94 DRAW_LIMB(user, right_hip, right_knee);
95 DRAW_LIMB(user, right_knee, right_foot);
97 DRAW_LIMB(user, left_hip, right_hip);
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) {
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],
113 float *com = i->second.skel_if->com();
114 glVertex4f(com[0], com[1], com[2], 1.0);
120 draw_user(i->second);
121 glColor4f(1, 1, 1, 1);
148 switch (print_state_) {
156 SkelGuiSkeletonDrawer3D::draw_circle(
unsigned int id,
float *p,
float radius)
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],
164 for (
int i = 0; i < 360; ++i) {
167 glVertex3f(p[0] + cos(rad) * radius, p[1] + sin(rad) * radius, p[2]);
169 glColor4f(1, 1, 1, 1);
179 print_state_ = state;