Fawkes API  Fawkes Development Version
skel_drawer.cpp
00001 
00002 /***************************************************************************
00003  *  skel_drawer.cpp - OpenNI Visualization: 3D skeleton drawer
00004  *
00005  *  Created: Sat Apr 02 20:00:50 2011
00006  *  Copyright  2006-2011  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #include "skel_drawer.h"
00024 #include <plugins/openni/utils/colors.h>
00025 
00026 #include <utils/math/angle.h>
00027 
00028 #include <cstring>
00029 #include <cstdio>
00030 #include <GL/glut.h>
00031 
00032 using namespace fawkes;
00033 using namespace fawkes::openni;
00034 
00035 /** @class SkelGuiSkeletonDrawer3D "skel_drawer.h"
00036  * Draw body skeleton using OpenGL (3D).
00037  * This class draws the limbs as read from the user interfaces. This version
00038  * draws in 3D and does not use the 2D projection.
00039  * @author Tim Niemueller
00040  */
00041 
00042 /** Constructor.
00043  * @param users map of users shared with interface observer
00044  * @param hands map of hands shared with interface observer
00045  */
00046 SkelGuiSkeletonDrawer3D::SkelGuiSkeletonDrawer3D(UserMap &users, HandMap &hands)
00047   : __users(users), __hands(hands)
00048 {
00049   __print_state = PRINT_ID_STATE;
00050 }
00051 
00052 void
00053 SkelGuiSkeletonDrawer3D::draw_limb(float *p1, float conf1,
00054                                  float *p2, float conf2)
00055 {
00056   if (conf1 < 0.5 || conf2 < 0.5)  return;
00057 
00058   //printf("Drawing from (%f,%f,%f) -> (%f,%f,%f)\n",
00059   //     p1[0], p1[1], p1[2], p2[0], p2[1], p2[2]);
00060   glVertex4f(p1[0], p1[1], p1[2], 1);
00061   glVertex4f(p2[0], p2[1], p2[2], 1);
00062 }
00063 
00064 #define DRAW_LIMB(user, joint1, joint2)                                 \
00065   draw_limb(user.skel_if->pos_##joint1(),                               \
00066             user.skel_if->pos_##joint1##_confidence(),                  \
00067             user.skel_if->pos_##joint2(),                               \
00068             user.skel_if->pos_##joint2##_confidence());
00069 
00070 void
00071 SkelGuiSkeletonDrawer3D::draw_user(UserInfo &user)
00072 {
00073   if (user.skel_if->state() != HumanSkeletonInterface::STATE_TRACKING)  return;
00074 
00075   DRAW_LIMB(user, head, neck);
00076   
00077   DRAW_LIMB(user, neck, left_shoulder);
00078   DRAW_LIMB(user, left_shoulder, left_elbow);
00079   DRAW_LIMB(user, left_elbow, left_hand);
00080   
00081   DRAW_LIMB(user, neck, right_shoulder);
00082   DRAW_LIMB(user, right_shoulder, right_elbow);
00083   DRAW_LIMB(user, right_elbow, right_hand);
00084 
00085   DRAW_LIMB(user, left_shoulder, torso);
00086   DRAW_LIMB(user, right_shoulder, torso);
00087 
00088   DRAW_LIMB(user, torso, left_hip);
00089   DRAW_LIMB(user, left_hip, left_knee);
00090   DRAW_LIMB(user, left_knee, left_foot);
00091 
00092   DRAW_LIMB(user, torso, right_hip);
00093   DRAW_LIMB(user, right_hip, right_knee);
00094   DRAW_LIMB(user, right_knee, right_foot);
00095 
00096   DRAW_LIMB(user, left_hip, right_hip);
00097 
00098 }
00099 
00100 /** Draw skeletons. */
00101 void
00102 SkelGuiSkeletonDrawer3D::draw()
00103 {
00104   for (UserMap::iterator i = __users.begin(); i != __users.end(); ++i) {
00105     i->second.skel_if->read();
00106     if (i->second.skel_if->state() != HumanSkeletonInterface::STATE_INVALID) {
00107       glPointSize(10);
00108       glBegin(GL_POINTS);
00109       glColor4f(1 - USER_COLORS[i->second.skel_if->user_id() % NUM_USER_COLORS][0],
00110                 1 - USER_COLORS[i->second.skel_if->user_id() % NUM_USER_COLORS][1],
00111                 1 - USER_COLORS[i->second.skel_if->user_id() % NUM_USER_COLORS][2],
00112                 1);
00113       float *com = i->second.skel_if->com();
00114       glVertex4f(com[0], com[1], com[2], 1.0);
00115       glEnd();
00116       glPointSize(1);
00117 
00118       glLineWidth(3);
00119       glBegin(GL_LINES);
00120       draw_user(i->second);
00121       glColor4f(1, 1, 1, 1);
00122       glEnd();
00123       glLineWidth(1);
00124     }
00125   }
00126 
00127   /*
00128   glEnable(GL_LINE_SMOOTH);
00129   glLineWidth(4);
00130   for (HandMap::iterator i = __hands.begin(); i != __hands.end(); ++i) {
00131     if (i->second.hand_if->is_visible()) {
00132       float proj[2] = {i->second.hand_if->world_x(), i->second.hand_if->world_y()};
00133       draw_circle(i->second.hand_if->world_z(), proj, 10);
00134     }
00135   }
00136   glLineWidth(1.);
00137   glDisable(GL_LINE_SMOOTH);
00138   */
00139 }
00140 
00141 /** Toggle the printing state.
00142  * This toggles through the printing state in the order PRINT_NONE,
00143  * PRINT_ID_STATE, and PRINT_ID.
00144  */
00145 void
00146 SkelGuiSkeletonDrawer3D::toggle_print_state()
00147 {
00148   switch (__print_state) {
00149   case PRINT_NONE:      __print_state = PRINT_ID_STATE; break;
00150   case PRINT_ID_STATE:  __print_state = PRINT_ID;       break;
00151   case PRINT_ID:        __print_state = PRINT_NONE;     break;
00152   }
00153 }
00154 
00155 
00156 void
00157 SkelGuiSkeletonDrawer3D::draw_circle(unsigned int id, float *p, float radius)
00158 {
00159   glBegin(GL_LINE_LOOP);
00160   glVertex3f(p[0], p[1], p[2]);
00161   glColor4f(1 - USER_COLORS[id % NUM_USER_COLORS][0],
00162             1 - USER_COLORS[id % NUM_USER_COLORS][1],
00163             1 - USER_COLORS[id % NUM_USER_COLORS][2],
00164             1);
00165   for (int i=0; i < 360; ++i) {
00166     float rad = deg2rad(i);;
00167     glVertex3f( p[0] + cos(rad) * radius, p[1] + sin(rad) * radius, p[2]);
00168   }
00169   glColor4f(1, 1, 1, 1);
00170   glEnd();
00171 }
00172 
00173 
00174 /** Set print state.
00175  * @param state new print state
00176  */
00177 void
00178 SkelGuiSkeletonDrawer3D::set_print_state(SkelGuiSkeletonDrawer3D::PrintState state)
00179 {
00180   __print_state = state;
00181 }