GLsceneBase.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 #include <fstream>
00003 #include <cstdio>
00004 #include <math.h>
00005 #include <GL/glew.h>
00006 #ifdef __APPLE__
00007 #include <GLUT/glut.h>
00008 #else
00009 #include <GL/glut.h>
00010 #endif
00011 #include <hrpModel/ModelLoaderUtil.h>
00012 #include <hrpModel/Sensor.h>
00013 #include <hrpModel/Light.h>
00014 #include "GLcamera.h"
00015 #include "GLbody.h"
00016 #include "GLlink.h"
00017 #include "GLutil.h"
00018 #include "GLsceneBase.h"
00019 #include "LogManagerBase.h"
00020 
00021 static void drawString(const char *str)
00022 {
00023     for (unsigned int i=0; i<strlen(str); i++){
00024         glutBitmapCharacter(GLUT_BITMAP_9_BY_15, str[i]);
00025     }
00026 }
00027 
00028 GLsceneBase::GLsceneBase(LogManagerBase *i_log) : 
00029     m_showingStatus(false), m_showSlider(false),
00030     m_width(DEFAULT_W), m_height(DEFAULT_H),
00031     m_videoWriter(NULL), m_cvImage(NULL), m_log(i_log),
00032     m_showFloorGrid(true), m_showInfo(true), m_defaultLights(true),
00033     m_request(REQ_NONE), 
00034     m_maxEdgeLen(0),
00035     m_targetObject(-1),
00036     m_isCapturing(false)
00037 {
00038     m_default_camera = new GLcamera(DEFAULT_W, DEFAULT_H, 0.1, 100.0, 30*M_PI/180);
00039     m_default_camera->setViewPoint(4,0,0.8);
00040     m_default_camera->setViewTarget(0,0,0.8);
00041     m_camera = m_default_camera;
00042     m_sem = SDL_CreateSemaphore(0);
00043     m_bgColor[0] = m_bgColor[1] = m_bgColor[2] = 0.0;
00044 
00045     hrp::Light::nextId = 2;
00046 }
00047 
00048 GLsceneBase::~GLsceneBase()
00049 {
00050     SDL_DestroySemaphore(m_sem);
00051     delete m_default_camera;
00052 }
00053 
00054 
00055 void GLsceneBase::setScreenSize(int w, int h){
00056     m_width = w;
00057     m_height = h;
00058 }
00059 
00060 void GLsceneBase::setCamera(GLcamera *i_camera)
00061 {
00062     if (!i_camera) return;
00063 
00064     m_camera = i_camera;
00065 }
00066 
00067 void GLsceneBase::nextCamera()
00068 {
00069     bool found = m_camera == m_default_camera ? true : false;
00070     for (unsigned int i=0; i<numBodies(); i++){
00071         hrp::BodyPtr b = body(i);
00072         for (unsigned int j=0; j<b->numLinks(); j++){
00073             GLlink *l = dynamic_cast<GLlink *>(b->link(j));
00074             const std::vector<GLcamera *>& cameras = l->cameras();
00075             for (size_t k=0; k<cameras.size(); k++){
00076                 if (cameras[k] == m_camera){
00077                     found = true;
00078                 }else if(found){
00079                     m_camera = cameras[k];
00080                     return;
00081                 }
00082             }
00083         } 
00084     }
00085     m_camera = m_default_camera;
00086 }
00087 
00088 void GLsceneBase::nextObject()
00089 {
00090     m_targetObject++;
00091     if (m_targetObject == (int)numBodies()) m_targetObject = -1;
00092 }
00093 
00094 GLcamera *GLsceneBase::getCamera()
00095 {
00096     return m_camera;
00097 }
00098 
00099 GLcamera *GLsceneBase::getDefaultCamera()
00100 {
00101     return m_default_camera;
00102 }
00103 
00104 void GLsceneBase::save(const char *i_fname)
00105 {
00106     char pixels[m_width*m_height*3];
00107 
00108     glReadBuffer(GL_BACK);
00109     glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
00110     glReadPixels(0,0, m_width,m_height,GL_RGB,GL_UNSIGNED_BYTE, pixels);
00111 
00112     std::ofstream ofs(i_fname, std::ios::out | std::ios::trunc | std::ios::binary );
00113     char buf[10];
00114     sprintf(buf, "%d %d", m_width, m_height);
00115     ofs << "P6" << std::endl << buf << std::endl << "255" << std::endl;
00116     for (int i=m_height-1; i>=0; i--){
00117         ofs.write((char *)(pixels+i*m_width*3), m_width*3);
00118     }
00119 }
00120 
00121 void GLsceneBase::capture(char *o_buffer)
00122 {
00123     glReadBuffer(GL_BACK);
00124     glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
00125     char *buf  = new char[m_width*m_height*3];
00126     glReadPixels(0,0, m_width,m_height,GL_BGR,GL_UNSIGNED_BYTE, buf);
00127     char *dst = o_buffer, *src;
00128     for (int i=0; i<m_height; i++){
00129         src = buf + (m_height -1 - i)*m_width*3;
00130         memcpy(dst, src, m_width*3);
00131         dst += m_width*3;
00132     }
00133     delete [] buf;
00134 }
00135 
00136 void GLsceneBase::initLights()
00137 {
00138     GLfloat light0pos[] = { 40.0, 60.0, 60.0, 1.0 };
00139     GLfloat light1pos[] = { -40.0, -60.0, -60.0, 1.0 };
00140     GLfloat light0col[] = { 0.9, 0.9, 1.0, 1.0 };
00141     GLfloat light1col[] = { 0.5, 0.4, 0.4, 1.0 };
00142 
00143     glEnable(GL_LIGHTING);
00144     glEnable(GL_LIGHT0);
00145     glEnable(GL_LIGHT1);
00146     glLightfv(GL_LIGHT0, GL_DIFFUSE, light0col);
00147     //glLightfv(GL_LIGHT0, GL_SPECULAR, light0col);
00148     glLightfv(GL_LIGHT1, GL_DIFFUSE, light1col);
00149     //glLightfv(GL_LIGHT1, GL_SPECULAR, light1col);
00150     glLightfv(GL_LIGHT0, GL_POSITION, light0pos);
00151     glLightfv(GL_LIGHT1, GL_POSITION, light1pos);
00152 }
00153 
00154 void GLsceneBase::defaultLights(bool flag)
00155 {
00156     if (m_defaultLights == flag) return;
00157 
00158     m_defaultLights = flag;
00159     if (flag){
00160         glEnable(GL_LIGHT0);
00161         glEnable(GL_LIGHT1);
00162     }else{
00163         glDisable(GL_LIGHT0);
00164         glDisable(GL_LIGHT1);
00165     }
00166 }
00167 
00168 bool GLsceneBase::defaultLights()
00169 {
00170     return m_defaultLights;
00171 }
00172 
00173 void GLsceneBase::init()
00174 {
00175     setCamera(m_default_camera);
00176 
00177     glewInit();
00178     initLights();
00179 
00180     glClearColor(m_bgColor[0], m_bgColor[1], m_bgColor[2], 0.0);
00181     glEnable(GL_DEPTH_TEST);
00182 
00183     glEnable(GL_CULL_FACE);
00184     glCullFace(GL_BACK);
00185 
00186     glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
00187     glEnable(GL_BLEND);
00188 }
00189 
00190 void GLsceneBase::showFloorGrid(bool flag)
00191 {
00192     m_showFloorGrid = flag;
00193 }
00194 
00195 bool GLsceneBase::showFloorGrid()
00196 {
00197     return m_showFloorGrid;
00198 }
00199 
00200 void GLsceneBase::drawFloorGrid()
00201 {
00202     glBegin(GL_LINES);
00203 
00204     // floor grids
00205     glColor3f(1,1,1);
00206     double s[3], e[3];
00207     s[2] = e[2] = 0;
00208     s[0] = 10; e[0] = -10;
00209     for (int i=-10;i<=10; i++){
00210         s[1] = e[1] = i;
00211         glVertex3dv(s);
00212         glVertex3dv(e);
00213     }
00214     s[1] = 10; e[1] = -10;
00215     for (int i=-10;i<=10; i++){
00216         s[0] = e[0] = i;
00217         glVertex3dv(s);
00218         glVertex3dv(e);
00219     }
00220 
00221     glEnd();
00222 }
00223 
00224 void GLsceneBase::showInfo(bool flag)
00225 {
00226     m_showInfo = flag;
00227 }
00228 
00229 void GLsceneBase::drawInfo(double fps, size_t ntri)
00230 {
00231     glColor3d(1.0,1.0,1.0);
00232     int h = m_height-15;
00233     glRasterPos2f(10, h);
00234     char buf[256];
00235     double tm = m_log->currentTime();
00236     if (tm >= 0){
00237         sprintf(buf, "Time:%6.3f[s]" , tm);
00238     }else{
00239         sprintf(buf, "Time:------[s]");
00240     }
00241     drawString(buf);
00242     h -= 15;
00243     glRasterPos2f(10, h);
00244     sprintf(buf, "Playback x%6.3f", m_log->playRatio());
00245     drawString(buf);
00246     h -= 15;
00247     glRasterPos2f(10, h);
00248     sprintf(buf, "FPS %2.0f(%6zutris)", fps, ntri);
00249     drawString(buf);
00250     if (m_camera != m_default_camera){
00251         sprintf(buf, "Camera: %s.%s", 
00252                 m_camera->link()->body->name().c_str(), 
00253                 m_camera->name().c_str());
00254         h -= 15;
00255         glRasterPos2f(10, h);
00256         drawString(buf);
00257     }
00258     if (m_targetObject >=0 && m_targetObject < (int)numBodies()){
00259         sprintf(buf, "Target: %s", targetObject()->name().c_str());
00260         h -= 15;
00261         glRasterPos2f(10, h);
00262         drawString(buf);
00263     }
00264     for (unsigned int i=0; i<m_msgs.size(); i++){
00265         glRasterPos2f(10, (m_msgs.size()-i)*15);
00266         drawString(m_msgs[i].c_str());
00267     }
00268     showStatus();
00269 }
00270 
00271 size_t GLsceneBase::drawObjects(bool showSensors)
00272 {
00273     size_t ntri = 0;
00274     boost::function2<void, hrp::Body *, hrp::Sensor *> callback;
00275     for (unsigned int i=0; i<numBodies(); i++){
00276         GLbody *glbody = dynamic_cast<GLbody *>(body(i).get());
00277         if (!glbody) std::cout << "dynamic_cast failed" << std::endl;
00278         if (!showSensors) {
00279             callback = glbody->getSensorDrawCallback();
00280             glbody->setSensorDrawCallback(NULL);
00281         }
00282         ntri += glbody->draw();
00283         if (!showSensors) glbody->setSensorDrawCallback(callback);
00284 
00285     }
00286     return ntri;
00287 }
00288 
00289 void GLsceneBase::draw()
00290 {
00291     struct timeval tv;
00292     gettimeofday(&tv, NULL);
00293     double fps = 1.0/((tv.tv_sec - m_lastDraw.tv_sec)+(tv.tv_usec - m_lastDraw.tv_usec)/1e6);
00294     m_lastDraw = tv;
00295 
00296     if (m_request == REQ_CLEAR) {
00297         clear();
00298         m_request = REQ_NONE;
00299         SDL_SemPost(m_sem);
00300     }
00301 
00302     int index = m_log->updateIndex();
00303     
00304     updateScene();
00305 
00306     glMatrixMode(GL_MODELVIEW);
00307     glLoadIdentity();
00308 
00309     size_t ntri = drawObjects();
00310 
00311     glDisable(GL_LIGHTING);
00312 
00313     if (m_showFloorGrid) drawFloorGrid();
00314     drawAdditionalLines();
00315 
00316     // draw texts
00317     glMatrixMode(GL_PROJECTION);
00318     glLoadIdentity();
00319     gluOrtho2D(0, m_width, 0, m_height);
00320     glPushMatrix();
00321     glMatrixMode(GL_MODELVIEW);
00322     glLoadIdentity();
00323     if (m_showInfo) drawInfo(fps, ntri);
00324     if (m_showSlider){
00325         glColor4f(0.0,0.0,0.0, 0.5);
00326         glRectf(SLIDER_SIDE_MARGIN,10,m_width-SLIDER_SIDE_MARGIN,20);
00327         unsigned int len = m_log->length();
00328         if (len>1){
00329             int x = ((double)index)/(len-1)*(m_width-20)+10;
00330             glRectf(x-5,5,x+5,25);
00331         }
00332     }
00333     glPopMatrix();
00334     glEnable(GL_LIGHTING);
00335 
00336     if (m_log->isRecording() && !m_isCapturing && !m_videoWriter){
00337         m_videoWriter = cvCreateVideoWriter(
00338             "olv.avi",
00339             CV_FOURCC('D','I','V','X'),
00340             m_log->fps(),
00341             cvSize(m_width, m_height));
00342         m_cvImage = cvCreateImage(
00343             cvSize(m_width, m_height),
00344             IPL_DEPTH_8U, 3);
00345     }
00346     if(m_videoWriter){
00347         char *dst = m_cvImage->imageData;
00348         capture(dst);
00349         cvWriteFrame(m_videoWriter, m_cvImage);
00350     }
00351     if(m_isCapturing){
00352         char fname[64];
00353         sprintf(fname, "capture%05.2f.png", m_log->time());
00354         save(fname);
00355     }
00356     if (!m_log->isRecording()){
00357         if (m_videoWriter){
00358             cvReleaseVideoWriter(&m_videoWriter);
00359             cvReleaseImage(&m_cvImage);
00360             m_videoWriter = NULL;
00361             m_cvImage = NULL;
00362         }
00363         if (m_isCapturing) m_isCapturing = false;
00364     }
00365     if (m_request == REQ_CAPTURE){
00366         save(m_fname.c_str());
00367         m_request = REQ_NONE;
00368         SDL_SemPost(m_sem);
00369     }
00370 
00371     // offscreen redering
00372     for (unsigned int i=0; i<numBodies(); i++){
00373         hrp::BodyPtr b = body(i);
00374         for (unsigned int j=0; j<b->numLinks(); j++){
00375             GLlink *l = dynamic_cast<GLlink *>(b->link(j));
00376             const std::vector<GLcamera *>& cameras = l->cameras();
00377             for (size_t k=0; k<cameras.size(); k++){
00378                 hrp::VisionSensor *s = cameras[k]->sensor();
00379                 if (!s->isEnabled) continue;
00380                 if (s->nextUpdateTime < m_log->currentTime()){
00381                     cameras[k]->render(this);
00382                     s->nextUpdateTime += 1.0/s->frameRate;
00383                 } 
00384             }
00385         } 
00386     }
00387 }
00388 
00389 void GLsceneBase::requestClear()
00390 {
00391     m_request = REQ_CLEAR;
00392     SDL_SemWait(m_sem);
00393 }
00394 
00395 void GLsceneBase::requestCapture(const char *i_fname)
00396 {
00397     m_fname = i_fname;
00398     m_request = REQ_CAPTURE;
00399     SDL_SemWait(m_sem);
00400 }
00401 
00402 void GLsceneBase::clear()
00403 {
00404     clearBodies();
00405     m_camera = m_default_camera;
00406 }
00407 
00408 void GLsceneBase::setView()
00409 {
00410     glViewport(0,0,m_width, m_height);
00411     m_camera->setView(m_width, m_height);
00412 }
00413 
00414 void GLsceneBase::addBody(hrp::BodyPtr i_body)
00415 {
00416     if (m_maxEdgeLen){
00417         GLbody *glbody = dynamic_cast<GLbody *>(i_body.get());
00418         if (glbody) glbody->divideLargeTriangles(m_maxEdgeLen);
00419     }
00420     WorldBase::addBody(i_body);
00421 }
00422 
00423 void GLsceneBase::maxEdgeLen(double i_len)
00424 {
00425     m_maxEdgeLen = i_len;
00426 }
00427 
00428 hrp::BodyPtr GLsceneBase::targetObject()
00429 {
00430     if (m_targetObject >= 0 && m_targetObject < (int)numBodies()){
00431         return body(m_targetObject);
00432     }else{
00433         return hrp::BodyPtr();
00434     }
00435 }
00436 
00437 void GLsceneBase::setBackGroundColor(float rgb[3])
00438 {
00439     for (int i=0; i<3; i++) m_bgColor[i] = rgb[i];
00440 }
00441 
00442 hrp::Vector3 GLsceneBase::center()
00443 {
00444     hrp::Vector3 mi,ma,min,max;
00445     for (unsigned int i=0; i<numBodies(); i++){
00446         GLbody *glbody = dynamic_cast<GLbody *>(body(i).get());
00447         glbody->computeAABB(mi,ma);
00448         if (i==0){
00449             min = mi; max = ma;
00450         }else{
00451             for (int j=0; j<3; j++){
00452                 if (min[j] > mi[j]) min[j] = mi[j];
00453                 if (max[j] < ma[j]) max[j] = ma[j];
00454             }
00455         }
00456     }
00457     return (min+max)/2;
00458 }


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