GLsceneBase.cpp
Go to the documentation of this file.
1 #include <iostream>
2 #include <fstream>
3 #include <cstdio>
4 #include <math.h>
5 #include <GL/glew.h>
6 #ifdef __APPLE__
7 #include <GLUT/glut.h>
8 #else
9 #include <GL/glut.h>
10 #endif
11 #include <hrpModel/ModelLoaderUtil.h>
12 #include <hrpModel/Sensor.h>
13 #include <hrpModel/Light.h>
14 #include "GLcamera.h"
15 #include "GLbody.h"
16 #include "GLlink.h"
17 #include "GLutil.h"
18 #include "GLsceneBase.h"
19 #include "LogManagerBase.h"
20 
21 static void drawString(const char *str)
22 {
23  for (unsigned int i=0; i<strlen(str); i++){
24  glutBitmapCharacter(GLUT_BITMAP_9_BY_15, str[i]);
25  }
26 }
27 
29  m_showingStatus(false), m_showSlider(false),
30  m_width(DEFAULT_W), m_height(DEFAULT_H),
31  m_videoWriter(NULL), m_cvImage(NULL), m_log(i_log),
32  m_showFloorGrid(true), m_showInfo(true), m_defaultLights(true),
33  m_request(REQ_NONE),
34  m_maxEdgeLen(0),
35  m_targetObject(-1),
36  m_isCapturing(false)
37 {
38  m_default_camera = new GLcamera(DEFAULT_W, DEFAULT_H, 0.1, 100.0, 30*M_PI/180);
42  m_sem = SDL_CreateSemaphore(0);
43  m_bgColor[0] = m_bgColor[1] = m_bgColor[2] = 0.0;
44 
46 }
47 
49 {
50  SDL_DestroySemaphore(m_sem);
51  delete m_default_camera;
52 }
53 
54 
55 void GLsceneBase::setScreenSize(int w, int h){
56  m_width = w;
57  m_height = h;
58 }
59 
61 {
62  if (!i_camera) return;
63 
64  m_camera = i_camera;
65 }
66 
68 {
69  bool found = m_camera == m_default_camera ? true : false;
70  for (unsigned int i=0; i<numBodies(); i++){
71  hrp::BodyPtr b = body(i);
72  for (unsigned int j=0; j<b->numLinks(); j++){
73  GLlink *l = dynamic_cast<GLlink *>(b->link(j));
74  const std::vector<GLcamera *>& cameras = l->cameras();
75  for (size_t k=0; k<cameras.size(); k++){
76  if (cameras[k] == m_camera){
77  found = true;
78  }else if(found){
79  m_camera = cameras[k];
80  return;
81  }
82  }
83  }
84  }
86 }
87 
89 {
91  if (m_targetObject == (int)numBodies()) m_targetObject = -1;
92 }
93 
95 {
96  return m_camera;
97 }
98 
100 {
101  return m_default_camera;
102 }
103 
104 void GLsceneBase::save(const char *i_fname)
105 {
106  char pixels[m_width*m_height*3];
107 
108  glReadBuffer(GL_BACK);
109  glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
110  glReadPixels(0,0, m_width,m_height,GL_RGB,GL_UNSIGNED_BYTE, pixels);
111 
112  std::ofstream ofs(i_fname, std::ios::out | std::ios::trunc | std::ios::binary );
113  char buf[10];
114  sprintf(buf, "%d %d", m_width, m_height);
115  ofs << "P6" << std::endl << buf << std::endl << "255" << std::endl;
116  for (int i=m_height-1; i>=0; i--){
117  ofs.write((char *)(pixels+i*m_width*3), m_width*3);
118  }
119 }
120 
121 void GLsceneBase::capture(char *o_buffer)
122 {
123  glReadBuffer(GL_BACK);
124  glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
125  char *buf = new char[m_width*m_height*3];
126  glReadPixels(0,0, m_width,m_height,GL_BGR,GL_UNSIGNED_BYTE, buf);
127  char *dst = o_buffer, *src;
128  for (int i=0; i<m_height; i++){
129  src = buf + (m_height -1 - i)*m_width*3;
130  memcpy(dst, src, m_width*3);
131  dst += m_width*3;
132  }
133  delete [] buf;
134 }
135 
137 {
138  GLfloat light0pos[] = { 40.0, 60.0, 60.0, 1.0 };
139  GLfloat light1pos[] = { -40.0, -60.0, -60.0, 1.0 };
140  GLfloat light0col[] = { 0.9, 0.9, 1.0, 1.0 };
141  GLfloat light1col[] = { 0.5, 0.4, 0.4, 1.0 };
142 
143  glEnable(GL_LIGHTING);
144  glEnable(GL_LIGHT0);
145  glEnable(GL_LIGHT1);
146  glLightfv(GL_LIGHT0, GL_DIFFUSE, light0col);
147  //glLightfv(GL_LIGHT0, GL_SPECULAR, light0col);
148  glLightfv(GL_LIGHT1, GL_DIFFUSE, light1col);
149  //glLightfv(GL_LIGHT1, GL_SPECULAR, light1col);
150  glLightfv(GL_LIGHT0, GL_POSITION, light0pos);
151  glLightfv(GL_LIGHT1, GL_POSITION, light1pos);
152 }
153 
155 {
156  if (m_defaultLights == flag) return;
157 
158  m_defaultLights = flag;
159  if (flag){
160  glEnable(GL_LIGHT0);
161  glEnable(GL_LIGHT1);
162  }else{
163  glDisable(GL_LIGHT0);
164  glDisable(GL_LIGHT1);
165  }
166 }
167 
169 {
170  return m_defaultLights;
171 }
172 
174 {
176 
177  glewInit();
178  initLights();
179 
180  glClearColor(m_bgColor[0], m_bgColor[1], m_bgColor[2], 0.0);
181  glEnable(GL_DEPTH_TEST);
182 
183  glEnable(GL_CULL_FACE);
184  glCullFace(GL_BACK);
185 
186  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
187  glEnable(GL_BLEND);
188 }
189 
191 {
192  m_showFloorGrid = flag;
193 }
194 
196 {
197  return m_showFloorGrid;
198 }
199 
201 {
202  glBegin(GL_LINES);
203 
204  // floor grids
205  glColor3f(1,1,1);
206  double s[3], e[3];
207  s[2] = e[2] = 0;
208  s[0] = 10; e[0] = -10;
209  for (int i=-10;i<=10; i++){
210  s[1] = e[1] = i;
211  glVertex3dv(s);
212  glVertex3dv(e);
213  }
214  s[1] = 10; e[1] = -10;
215  for (int i=-10;i<=10; i++){
216  s[0] = e[0] = i;
217  glVertex3dv(s);
218  glVertex3dv(e);
219  }
220 
221  glEnd();
222 }
223 
224 void GLsceneBase::showInfo(bool flag)
225 {
226  m_showInfo = flag;
227 }
228 
229 void GLsceneBase::drawInfo(double fps, size_t ntri)
230 {
231  glColor3d(1.0,1.0,1.0);
232  int h = m_height-15;
233  glRasterPos2f(10, h);
234  char buf[256];
235  double tm = m_log->currentTime();
236  if (tm >= 0){
237  sprintf(buf, "Time:%6.3f[s]" , tm);
238  }else{
239  sprintf(buf, "Time:------[s]");
240  }
241  drawString(buf);
242  h -= 15;
243  glRasterPos2f(10, h);
244  sprintf(buf, "Playback x%6.3f", m_log->playRatio());
245  drawString(buf);
246  h -= 15;
247  glRasterPos2f(10, h);
248  sprintf(buf, "FPS %2.0f(%6zutris)", fps, ntri);
249  drawString(buf);
250  if (m_camera != m_default_camera){
251  sprintf(buf, "Camera: %s.%s",
252  m_camera->link()->body->name().c_str(),
253  m_camera->name().c_str());
254  h -= 15;
255  glRasterPos2f(10, h);
256  drawString(buf);
257  }
258  if (m_targetObject >=0 && m_targetObject < (int)numBodies()){
259  sprintf(buf, "Target: %s", targetObject()->name().c_str());
260  h -= 15;
261  glRasterPos2f(10, h);
262  drawString(buf);
263  }
264  for (unsigned int i=0; i<m_msgs.size(); i++){
265  glRasterPos2f(10, (m_msgs.size()-i)*15);
266  drawString(m_msgs[i].c_str());
267  }
268  showStatus();
269 }
270 
271 size_t GLsceneBase::drawObjects(bool showSensors)
272 {
273  size_t ntri = 0;
274  boost::function2<void, hrp::Body *, hrp::Sensor *> callback;
275  for (unsigned int i=0; i<numBodies(); i++){
276  GLbody *glbody = dynamic_cast<GLbody *>(body(i).get());
277  if (!glbody) std::cout << "dynamic_cast failed" << std::endl;
278  if (!showSensors) {
279  callback = glbody->getSensorDrawCallback();
280  glbody->setSensorDrawCallback(NULL);
281  }
282  ntri += glbody->draw();
283  if (!showSensors) glbody->setSensorDrawCallback(callback);
284 
285  }
286  return ntri;
287 }
288 
290 {
291  struct timeval tv;
292  gettimeofday(&tv, NULL);
293  double fps = 1.0/((tv.tv_sec - m_lastDraw.tv_sec)+(tv.tv_usec - m_lastDraw.tv_usec)/1e6);
294  m_lastDraw = tv;
295 
296  if (m_request == REQ_CLEAR) {
297  clear();
299  SDL_SemPost(m_sem);
300  }
301 
302  int index = m_log->updateIndex();
303 
304  updateScene();
305 
306  glMatrixMode(GL_MODELVIEW);
307  glLoadIdentity();
308 
309  size_t ntri = drawObjects();
310 
311  glDisable(GL_LIGHTING);
312 
315 
316  // draw texts
317  glMatrixMode(GL_PROJECTION);
318  glLoadIdentity();
319  gluOrtho2D(0, m_width, 0, m_height);
320  glPushMatrix();
321  glMatrixMode(GL_MODELVIEW);
322  glLoadIdentity();
323  if (m_showInfo) drawInfo(fps, ntri);
324  if (m_showSlider){
325  glColor4f(0.0,0.0,0.0, 0.5);
327  unsigned int len = m_log->length();
328  if (len>1){
329  int x = ((double)index)/(len-1)*(m_width-20)+10;
330  glRectf(x-5,5,x+5,25);
331  }
332  }
333  glPopMatrix();
334  glEnable(GL_LIGHTING);
335 
337  m_videoWriter = cvCreateVideoWriter(
338  "olv.avi",
339  CV_FOURCC('D','I','V','X'),
340  m_log->fps(),
341  cvSize(m_width, m_height));
342  m_cvImage = cvCreateImage(
343  cvSize(m_width, m_height),
344  IPL_DEPTH_8U, 3);
345  }
346  if(m_videoWriter){
347  char *dst = m_cvImage->imageData;
348  capture(dst);
349  cvWriteFrame(m_videoWriter, m_cvImage);
350  }
351  if(m_isCapturing){
352  char fname[64];
353  sprintf(fname, "capture%05.2f.png", m_log->time());
354  save(fname);
355  }
356  if (!m_log->isRecording()){
357  if (m_videoWriter){
358  cvReleaseVideoWriter(&m_videoWriter);
359  cvReleaseImage(&m_cvImage);
360  m_videoWriter = NULL;
361  m_cvImage = NULL;
362  }
363  if (m_isCapturing) m_isCapturing = false;
364  }
365  if (m_request == REQ_CAPTURE){
366  save(m_fname.c_str());
368  SDL_SemPost(m_sem);
369  }
370 
371  // offscreen redering
372  for (unsigned int i=0; i<numBodies(); i++){
373  hrp::BodyPtr b = body(i);
374  for (unsigned int j=0; j<b->numLinks(); j++){
375  GLlink *l = dynamic_cast<GLlink *>(b->link(j));
376  const std::vector<GLcamera *>& cameras = l->cameras();
377  for (size_t k=0; k<cameras.size(); k++){
378  hrp::VisionSensor *s = cameras[k]->sensor();
379  if (!s->isEnabled) continue;
380  if (s->nextUpdateTime < m_log->currentTime()){
381  cameras[k]->render(this);
382  s->nextUpdateTime += 1.0/s->frameRate;
383  }
384  }
385  }
386  }
387 }
388 
390 {
392  SDL_SemWait(m_sem);
393 }
394 
395 void GLsceneBase::requestCapture(const char *i_fname)
396 {
397  m_fname = i_fname;
399  SDL_SemWait(m_sem);
400 }
401 
403 {
404  clearBodies();
406 }
407 
409 {
410  glViewport(0,0,m_width, m_height);
412 }
413 
415 {
416  if (m_maxEdgeLen){
417  GLbody *glbody = dynamic_cast<GLbody *>(i_body.get());
418  if (glbody) glbody->divideLargeTriangles(m_maxEdgeLen);
419  }
420  WorldBase::addBody(i_body);
421 }
422 
423 void GLsceneBase::maxEdgeLen(double i_len)
424 {
425  m_maxEdgeLen = i_len;
426 }
427 
429 {
430  if (m_targetObject >= 0 && m_targetObject < (int)numBodies()){
431  return body(m_targetObject);
432  }else{
433  return hrp::BodyPtr();
434  }
435 }
436 
438 {
439  for (int i=0; i<3; i++) m_bgColor[i] = rgb[i];
440 }
441 
443 {
444  hrp::Vector3 mi,ma,min,max;
445  for (unsigned int i=0; i<numBodies(); i++){
446  GLbody *glbody = dynamic_cast<GLbody *>(body(i).get());
447  glbody->computeAABB(mi,ma);
448  if (i==0){
449  min = mi; max = ma;
450  }else{
451  for (int j=0; j<3; j++){
452  if (min[j] > mi[j]) min[j] = mi[j];
453  if (max[j] < ma[j]) max[j] = ma[j];
454  }
455  }
456  }
457  return (min+max)/2;
458 }
size_t draw()
Definition: GLbody.cpp:59
#define max(a, b)
bool defaultLights()
#define SLIDER_SIDE_MARGIN
Definition: GLsceneBase.h:28
void setCamera(GLcamera *i_camera)
Definition: GLsceneBase.cpp:60
virtual double time()=0
bool m_showFloorGrid
Definition: GLsceneBase.h:83
GLcamera * m_default_camera
Definition: GLsceneBase.h:77
void initLights()
void drawFloorGrid()
void divideLargeTriangles(double maxEdgeLen)
Definition: GLbody.cpp:94
virtual void updateScene()=0
GLsceneBase(LogManagerBase *i_log)
Definition: GLsceneBase.cpp:28
void setBackGroundColor(float rgb[3])
hrp::Vector3 center()
png_infop png_charpp name
boost::function2< void, hrp::Body *, hrp::Sensor * > getSensorDrawCallback()
Definition: GLbody.cpp:89
bool showFloorGrid()
double m_maxEdgeLen
Definition: GLsceneBase.h:86
std::vector< std::string > m_msgs
Definition: GLsceneBase.h:74
void nextCamera()
Definition: GLsceneBase.cpp:67
const std::string & name() const
Definition: GLcamera.cpp:55
GLcamera * getDefaultCamera()
Definition: GLsceneBase.cpp:99
void setViewTarget(double x, double y, double z)
Definition: GLcamera.cpp:103
bool m_isCapturing
Definition: GLsceneBase.h:89
png_uint_32 i
#define DEFAULT_H
Definition: GLsceneBase.h:26
void computeAABB(hrp::Vector3 &o_min, hrp::Vector3 &o_max)
Definition: GLbody.cpp:102
GLlink * link()
Definition: GLcamera.cpp:112
size_t drawObjects(bool showSensors=true)
boost::shared_ptr< Body > BodyPtr
void setView(int w, int h)
Definition: GLcamera.cpp:77
double playRatio()
Definition: GLbody.h:11
Eigen::Vector3d Vector3
float m_bgColor[3]
Definition: GLsceneBase.h:88
#define min(a, b)
BodyPtr body(int index)
int gettimeofday(struct timeval *tv, struct timezone *tz)
static int nextId
void setViewPoint(double x, double y, double z)
Definition: GLcamera.cpp:98
int m_targetObject
Definition: GLsceneBase.h:87
virtual int updateIndex()=0
IplImage * m_cvImage
Definition: GLsceneBase.h:80
void addBody(hrp::BodyPtr i_body)
std::string m_fname
Definition: GLsceneBase.h:85
const std::string & name()
virtual double currentTime()=0
list index
void save(const char *i_fname)
LogManagerBase * m_log
Definition: GLsceneBase.h:81
static void drawString(const char *str)
Definition: GLsceneBase.cpp:21
virtual void showStatus()
Definition: GLsceneBase.h:56
void setView()
bool m_showSlider
Definition: GLsceneBase.h:75
png_bytep buf
void requestCapture(const char *i_fname)
void setScreenSize(int w, int h)
Definition: GLsceneBase.cpp:55
SDL_sem * m_sem
Definition: GLsceneBase.h:82
hrp::BodyPtr targetObject()
#define M_PI
bool m_showInfo
Definition: GLsceneBase.h:83
bool m_defaultLights
Definition: GLsceneBase.h:83
#define DEFAULT_W
Definition: GLsceneBase.h:25
std::string sprintf(char const *__restrict fmt,...)
GLcamera * m_camera
Definition: GLsceneBase.h:77
void showInfo(bool flag)
void drawInfo(double fps, size_t ntri)
void maxEdgeLen(double i_len)
void setSensorDrawCallback(boost::function2< void, hrp::Body *, hrp::Sensor *> f)
Definition: GLbody.cpp:84
virtual unsigned int length()=0
void capture()
Definition: GLsceneBase.h:67
void requestClear()
void clearBodies()
virtual void drawAdditionalLines()
Definition: GLsceneBase.h:57
unsigned int numBodies()
virtual ~GLsceneBase()
Definition: GLsceneBase.cpp:48
CvVideoWriter * m_videoWriter
Definition: GLsceneBase.h:79
struct timeval m_lastDraw
Definition: GLsceneBase.h:78
void nextObject()
Definition: GLsceneBase.cpp:88
GLcamera * getCamera()
Definition: GLsceneBase.cpp:94


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