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
00148 glLightfv(GL_LIGHT1, GL_DIFFUSE, light1col);
00149
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
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
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
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 }