rtc/CollisionDetector/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 "TimedPosture.h"
15 #include "GLscene.h"
16 #include "hrpsys/idl/CollisionDetectorService.hh"
17 
18 
19 using namespace OpenHRP;
20 using namespace hrp;
21 using namespace CollisionDetectorComponent;
22 
23 static void drawString(const char *str)
24 {
25  for (unsigned int i=0; i<strlen(str); i++){
26  glutBitmapCharacter(GLUT_BITMAP_9_BY_15, str[i]);
27  }
28 }
29 
31 {
32  if (m_log->index()<0) return;
33 
34 #ifdef USE_COLLISION_STATE
37  GLbody *glbody = dynamic_cast<GLbody *>(body(0).get());
38  OpenHRP::CollisionDetectorService::CollisionState &co = lm->state();
39  if (co.angle.length() == glbody->numJoints()){
40  for (unsigned int i=0; i<glbody->numJoints(); i++){
41  GLlink *j = (GLlink *)glbody->joint(i);
42  if (j){
43  j->setQ(co.angle[i]);
44  }
45  }
46  }
47 #else
49  = (LogManager<TimedPosture> *)m_log;
50  GLbody *glbody = dynamic_cast<GLbody *>(body(0).get());
51  TimedPosture &ts = lm->state();
52  if (ts.posture.size() == glbody->numJoints()){
53  for (unsigned int i=0; i<glbody->numJoints(); i++){
54  GLlink *j = (GLlink *)glbody->joint(i);
55  if (j){
56  j->setQ(ts.posture[i]);
57  }
58  }
59  }
60 #endif
61 }
62 
64 {
65  if (m_log->index()<0) return;
66 
67 #ifdef USE_COLLISION_STATE
70  OpenHRP::CollisionDetectorService::CollisionState &co = lm->state();
71 
72  glBegin(GL_LINES);
73  glColor3f(1,0,0);
74  for (unsigned int i=0; i<co.lines.length(); i++){
75  GLdouble p0[3], p1[3];
76  p0[0] = co.lines[i][0][0];
77  p0[1] = co.lines[i][0][1];
78  p0[2] = co.lines[i][0][2];
79  p1[0] = co.lines[i][1][0];
80  p1[1] = co.lines[i][1][1];
81  p1[2] = co.lines[i][1][2];
82  glVertex3dv(p0);
83  glVertex3dv(p1);
84  }
85  glEnd();
86 
87  glPointSize(4.0);
88  glBegin(GL_POINTS);
89  glColor3f(1,0,0);
90  for (unsigned int i=0; i<co.lines.length(); i++){
91  GLdouble p0[3], p1[3];
92  p0[0] = co.lines[i][0][0];
93  p0[1] = co.lines[i][0][1];
94  p0[2] = co.lines[i][0][2];
95  p1[0] = co.lines[i][1][0];
96  p1[1] = co.lines[i][1][1];
97  p1[2] = co.lines[i][1][2];
98  glVertex3dv(p0);
99  glVertex3dv(p1);
100  }
101  glEnd();
102 #else
104  = (LogManager<TimedPosture> *)m_log;
105  TimedPosture &tp = lm->state();
106 
107  glBegin(GL_LINES);
108  glColor3f(1,0,0);
109  for (unsigned int i=0; i<tp.lines.size(); i++){
110  const std::pair<hrp::Vector3, hrp::Vector3>& line = tp.lines[i];
111  glVertex3dv(line.first.data());
112  glVertex3dv(line.second.data());
113  }
114  glEnd();
115 
116  glPointSize(4.0);
117  glBegin(GL_POINTS);
118  glColor3f(1,0,0);
119  for (unsigned int i=0; i<tp.lines.size(); i++){
120  const std::pair<hrp::Vector3, hrp::Vector3>& line = tp.lines[i];
121  glVertex3dv(line.first.data());
122  glVertex3dv(line.second.data());
123  }
124  glEnd();
125 #endif
126 }
127 
129 {
130  char buf[256];
131 
132  GLbody *glbody = dynamic_cast<GLbody *>(body(0).get());
133  int width = m_width - 220;
134 #define HEIGHT_STEP 12
135  int height = m_height-HEIGHT_STEP;
136  int x = width;
137 
138  for (unsigned int i=0; i<glbody->numLinks(); i++){
139  hrp::Link *l = glbody->link(i);
140  if (l){
141  sprintf(buf, "%13s %4d tris",
142  l->name.c_str(),
143  l->coldetModel->getNumTriangles());
144  glRasterPos2f(x, height);
145  drawString(buf);
146  height -= HEIGHT_STEP;
147  }
148  }
149 
150  if (m_log->index()<0) return;
151 
154  OpenHRP::CollisionDetectorService::CollisionState &co = lm->state();
155 
156  height -= HEIGHT_STEP;
157 
158  x = width - 34;
159  sprintf(buf, "Number of pair %8d", co.lines.length());
160  glRasterPos2f(x, height);
161  drawString(buf);
162  height -= HEIGHT_STEP;
163 
164  sprintf(buf, "Calc Time [msec] %8.3f", co.computation_time);
165  glRasterPos2f(x, height);
166  drawString(buf);
167  height -= HEIGHT_STEP;
168 
169  sprintf(buf, "Recover Time[msec] %8.3f", co.recover_time);
170  glRasterPos2f(x, height);
171  drawString(buf);
172  height -= HEIGHT_STEP;
173 
174  sprintf(buf, "Safe Posture %8s", co.safe_posture?"true":"false");
175  glRasterPos2f(x, height);
176  drawString(buf);
177  height -= HEIGHT_STEP;
178 
179  sprintf(buf, "Loop for check %8d", co.loop_for_check);
180  glRasterPos2f(x, height);
181  drawString(buf);
182  height -= HEIGHT_STEP;
183 
184 }
185 
186 
unsigned int numJoints() const
unsigned int numLinks() const
void drawAdditionalLines()
void updateScene()
Link * link(int index) const
png_uint_32 i
png_infop png_uint_32 * width
static void drawString(const char *str)
Definition: GLbody.h:11
png_infop png_uint_32 png_uint_32 * height
png_bytep buf
T & state()
Definition: LogManager.h:140
std::string sprintf(char const *__restrict fmt,...)
std::vector< std::pair< hrp::Vector3, hrp::Vector3 > > lines
Definition: TimedPosture.h:8
Link * joint(int id) const
#define HEIGHT_STEP
std::vector< double > posture
Definition: TimedPosture.h:7


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:20