GLscene.cpp
Go to the documentation of this file.
00001 #include <cstdio>
00002 #include <iostream>
00003 #include <fstream>
00004 #ifdef __APPLE__
00005 #include <GLUT/glut.h>
00006 #else
00007 #include <GL/glut.h>
00008 #endif
00009 #include <sys/time.h>
00010 #include "hrpsys/util/GLcamera.h"
00011 #include "hrpsys/util/GLlink.h"
00012 #include "hrpsys/util/GLbody.h"
00013 #include "hrpsys/util/LogManager.h"
00014 #include "hrpsys/idl/HRPDataTypes.hh"
00015 #include "GLscene.h"
00016 
00017 using namespace OpenHRP;
00018 using namespace hrp;
00019 
00020 void drawString2(const char *str)
00021 {
00022     for (unsigned int i=0; i<strlen(str); i++){
00023         glutBitmapCharacter(GLUT_BITMAP_8_BY_13, str[i]);
00024     }
00025 }
00026 
00027 void white(){
00028     glColor3d(1.0,1.0,1.0);
00029 }
00030 
00031 void red(){
00032     glColor3d(1.0,0.0,0.0);
00033 }
00034 
00035 void yellow(){
00036     glColor3d(1.0,1.0,0.0);
00037 }
00038 
00039 void green(){
00040     glColor3d(0.0,1.0,0.0);
00041 }
00042 
00043 void blue(){
00044     glColor3d(0.0,0.0,1.0);
00045 }
00046 
00047 void black(){
00048     glColor3d(0.0,0.0,0.0);
00049 }
00050 
00051 void GLscene::updateScene()
00052 {
00053     if (m_log->index()<0) return;
00054 
00055     LogManager<OpenHRP::SceneState> *lm 
00056         = (LogManager<OpenHRP::SceneState> *)m_log;
00057     OpenHRP::SceneState &ss = lm->state();
00058     for (unsigned int i=0; i<ss.states.length(); i++){
00059         OpenHRP::RobotState &rs = ss.states[i];
00060         double pos[] = {rs.basePose.position.x,
00061                         rs.basePose.position.y,
00062                         rs.basePose.position.z};
00063         double rpy[] = {rs.basePose.orientation.r,
00064                         rs.basePose.orientation.p,
00065                         rs.basePose.orientation.y};
00066         GLbody *glbody = dynamic_cast<GLbody *>(body(i).get());
00067         glbody->setPosture(rs.q.get_buffer(), pos, rpy);
00068     }
00069 }
00070 
00071 void GLscene::showStatus()
00072 {
00073     if (m_log->index()<0) return;
00074 
00075     LogManager<OpenHRP::SceneState> *lm 
00076         = (LogManager<OpenHRP::SceneState> *)m_log;
00077     OpenHRP::SceneState &sstate = lm->state();
00078 
00079     if (m_showingStatus){
00080         GLbody *glbody = NULL;
00081         OpenHRP::RobotState *rstate = NULL;
00082         for (unsigned int i=0; i<numBodies(); i++){
00083             if (body(i)->numJoints()){
00084                 glbody = dynamic_cast<GLbody *>(body(i).get());
00085                 rstate = &sstate.states[i];
00086                 break;
00087             }
00088         }
00089 #define HEIGHT_STEP 12
00090         int width = m_width - 410;
00091         int height = m_height-HEIGHT_STEP;
00092         char buf[256];
00093         for (unsigned int i=0; i<glbody->numJoints(); i++){
00094             hrp::Link *l = glbody->joint(i);
00095             if (l){
00096                 int x = width;
00097                 // joint ID
00098                 sprintf(buf, "%2d",i);
00099                 glRasterPos2f(x, height);
00100                 drawString2(buf);
00101                 white();
00102                 x += 8*3;
00103                 // joint name, current angle
00104                 sprintf(buf, "%13s %8.3f", 
00105                         l->name.c_str(), 
00106                         rstate->q[i]*180/M_PI);
00107                 glRasterPos2f(x, height);
00108                 drawString2(buf);
00109                 x += 8*(14+9);
00110                 
00111                 height -= HEIGHT_STEP;
00112             }
00113         }
00114         glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
00115         glEnable(GL_BLEND);
00116         glColor4f(0.0,0.0,0.0, 0.5);
00117         if (m_showSlider){
00118             glRectf(width,SLIDER_AREA_HEIGHT,m_width,m_height);
00119         }else{
00120             glRectf(width,0,m_width,m_height);
00121         }
00122         glDisable(GL_BLEND);
00123     }
00124 }
00125 
00126 void printMatrix(double mat[16])
00127 {
00128     for (int i=0; i<4; i++){
00129         for (int j=0; j<4; j++){
00130             printf("%6.3f ", mat[j*4+i]);
00131         }
00132         printf("\n");
00133     }
00134 }
00135 


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:55