rtc/VirtualCamera/GLscene.cpp
Go to the documentation of this file.
1 #include <cstdio>
2 #include <iostream>
3 #include <fstream>
4 #ifdef __APPLE__
5 #include <GLUT/glut.h>
6 #else
7 #include <GL/glut.h>
8 #endif
9 #include <sys/time.h>
10 #include "hrpsys/util/GLcamera.h"
11 #include "hrpsys/util/GLlink.h"
12 #include "hrpsys/util/GLbody.h"
13 #include "hrpsys/util/LogManager.h"
14 #include "hrpsys/idl/HRPDataTypes.hh"
15 #include "GLscene.h"
16 
17 using namespace OpenHRP;
18 using namespace hrp;
19 
20 void drawString2(const char *str)
21 {
22  for (unsigned int i=0; i<strlen(str); i++){
23  glutBitmapCharacter(GLUT_BITMAP_8_BY_13, str[i]);
24  }
25 }
26 
27 void white(){
28  glColor3d(1.0,1.0,1.0);
29 }
30 
31 void red(){
32  glColor3d(1.0,0.0,0.0);
33 }
34 
35 void yellow(){
36  glColor3d(1.0,1.0,0.0);
37 }
38 
39 void green(){
40  glColor3d(0.0,1.0,0.0);
41 }
42 
43 void blue(){
44  glColor3d(0.0,0.0,1.0);
45 }
46 
47 void black(){
48  glColor3d(0.0,0.0,0.0);
49 }
50 
52 {
53  if (m_log->index()<0) return;
54 
57  OpenHRP::SceneState &ss = lm->state();
58  for (unsigned int i=0; i<ss.states.length(); i++){
59  OpenHRP::RobotState &rs = ss.states[i];
60  double pos[] = {rs.basePose.position.x,
61  rs.basePose.position.y,
62  rs.basePose.position.z};
63  double rpy[] = {rs.basePose.orientation.r,
64  rs.basePose.orientation.p,
65  rs.basePose.orientation.y};
66  GLbody *glbody = dynamic_cast<GLbody *>(body(i).get());
67  glbody->setPosture(rs.q.get_buffer(), pos, rpy);
68  }
69 }
70 
72 {
73  if (m_log->index()<0) return;
74 
77  OpenHRP::SceneState &sstate = lm->state();
78 
79  if (m_showingStatus){
80  GLbody *glbody = NULL;
81  OpenHRP::RobotState *rstate = NULL;
82  for (unsigned int i=0; i<numBodies(); i++){
83  if (body(i)->numJoints()){
84  glbody = dynamic_cast<GLbody *>(body(i).get());
85  rstate = &sstate.states[i];
86  break;
87  }
88  }
89 #define HEIGHT_STEP 12
90  int width = m_width - 410;
91  int height = m_height-HEIGHT_STEP;
92  char buf[256];
93  for (unsigned int i=0; i<glbody->numJoints(); i++){
94  hrp::Link *l = glbody->joint(i);
95  if (l){
96  int x = width;
97  // joint ID
98  sprintf(buf, "%2d",i);
99  glRasterPos2f(x, height);
100  drawString2(buf);
101  white();
102  x += 8*3;
103  // joint name, current angle
104  sprintf(buf, "%13s %8.3f",
105  l->name.c_str(),
106  rstate->q[i]*180/M_PI);
107  glRasterPos2f(x, height);
108  drawString2(buf);
109  x += 8*(14+9);
110 
111  height -= HEIGHT_STEP;
112  }
113  }
114  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
115  glEnable(GL_BLEND);
116  glColor4f(0.0,0.0,0.0, 0.5);
117  if (m_showSlider){
118  glRectf(width,SLIDER_AREA_HEIGHT,m_width,m_height);
119  }else{
120  glRectf(width,0,m_width,m_height);
121  }
122  glDisable(GL_BLEND);
123  }
124 }
125 
126 void printMatrix(double mat[16])
127 {
128  for (int i=0; i<4; i++){
129  for (int j=0; j<4; j++){
130  printf("%6.3f ", mat[j*4+i]);
131  }
132  printf("\n");
133  }
134 }
135 
void drawString2(const char *str)
void updateScene()
#define HEIGHT_STEP
png_uint_32 i
png_infop png_uint_32 * width
Definition: GLbody.h:11
png_infop png_uint_32 png_uint_32 * height
def j(str, encoding="cp932")
png_bytep buf
Link * joint(int id) const
T & state()
Definition: LogManager.h:140
#define M_PI
void setPosture(const double *i_angles)
Definition: GLbody.cpp:21
std::string sprintf(char const *__restrict fmt,...)
unsigned int numJoints() const
#define SLIDER_AREA_HEIGHT
Definition: GLsceneBase.h:22
void printMatrix(double mat[16])


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Thu May 6 2021 02:41:50