SDLUtil.cpp
Go to the documentation of this file.
00001 #include <math.h>
00002 #ifdef __APPLE__
00003 #include <OpenGL/glu.h>
00004 #include <GLUT/glut.h>
00005 #else
00006 #include <GL/glu.h>
00007 #include <GL/glut.h>
00008 #endif
00009 #include <SDL.h>
00010 #include "hrpsys/util/ThreadedObject.h"
00011 #include "hrpsys/util/LogManagerBase.h"
00012 #include "hrpsys/util/GLsceneBase.h"
00013 #include "hrpsys/util/GLcamera.h"
00014 #include "hrpsys/util/GLlink.h"
00015 #include "SDLUtil.h"
00016 
00017 
00018 SDLwindow::SDLwindow(GLsceneBase* i_scene, LogManagerBase *i_log,
00019                      ThreadedObject* i_throbj) :
00020     scene(i_scene), log(i_log), throbj(i_throbj),
00021     width(640), height(480),
00022     pan(M_PI/4), tilt(M_PI/16), radius(5),
00023     isShiftPressed(false), isControlPressed(false),
00024     xCenter(0), yCenter(0), zCenter(0.8),
00025     showingHelp(false), initialized(false)
00026 {
00027     helpcommand.push_back("h: help");
00028     instructions.push_back("q: quit");
00029     instructions.push_back("SPACE: play/stop");
00030     instructions.push_back("f: faster");
00031     instructions.push_back("s: slower");
00032     instructions.push_back("r: record movie");
00033     instructions.push_back("t: toggle robot state");
00034     instructions.push_back("d: rotate draw mode");
00035     instructions.push_back("n: select next camera");
00036     instructions.push_back("c: clear scene");
00037     instructions.push_back("g: toggle floor grid");
00038     instructions.push_back("l: toggle default lights");
00039     instructions.push_back("o: rotate target object");
00040     instructions.push_back("v: view center of objects");
00041     if (throbj){
00042         instructions.push_back("p: pause/resume background thread");
00043     }
00044     scene->setMessages(helpcommand);
00045 }
00046 
00047 SDLwindow::~SDLwindow()
00048 {
00049     if ( initialized ) {
00050         SDL_Quit();
00051     }
00052 }
00053 
00054 bool SDLwindow::init(int w, int h, bool resizable)
00055 {
00056     if (w) width = w;
00057     if (h) height = h;
00058 
00059     int argc=1;
00060     char *argv[] = {(char *)"dummy"};
00061     glutInit(&argc, argv); // for bitmap fonts
00062 
00063     if(SDL_Init(SDL_INIT_VIDEO)<0) {
00064         fprintf(stderr,"failed to initialize SDL.\n");
00065         return false;
00066     }
00067 
00068     SDL_GL_SetAttribute(SDL_GL_DOUBLEBUFFER,1);
00069     SDL_GL_SetAttribute(SDL_GL_SWAP_CONTROL,1);
00070     SDL_Surface *screen;
00071     int flag = SDL_HWSURFACE | SDL_GL_DOUBLEBUFFER | SDL_OPENGL;
00072     if (resizable) flag |= SDL_RESIZABLE;
00073     screen=SDL_SetVideoMode(width,height,32,flag);
00074     if(!screen) {
00075         fprintf(stderr,"failed to set video mode to %dx%dx32.\n",width,height);
00076         SDL_Quit();
00077         return false;
00078     }
00079     SDL_WM_SetCaption("hrpsys viewer", NULL);
00080     SDL_EnableKeyRepeat(SDL_DEFAULT_REPEAT_DELAY,SDL_DEFAULT_REPEAT_INTERVAL);
00081 
00082     scene->init();
00083     scene->setScreenSize(width, height);
00084 
00085     initialized = true;
00086     return true;
00087 }
00088 
00089 double SDLwindow::sliderRatio(double x)
00090 {
00091     double ratio = (x - SLIDER_SIDE_MARGIN)/(width - SLIDER_SIDE_MARGIN*2);
00092     if (ratio < 0.0) ratio = 0.0;
00093     if (ratio > 1.0) ratio = 1.0;
00094     return ratio;
00095 }
00096 
00097 bool SDLwindow::processEvents()
00098 {
00099     SDL_Event event;
00100     while(SDL_PollEvent(&event)){
00101         switch(event.type){
00102         case SDL_QUIT:
00103             return false;
00104         case SDL_KEYDOWN:
00105         {
00106             //printf("%d\n", event.key.keysym.sym);
00107             int delta = isShiftPressed ? 10 : 1;
00108             switch(event.key.keysym.sym){
00109             case SDLK_h:
00110                 if (showingHelp){
00111                     scene->setMessages(helpcommand);
00112                 }else{
00113                     scene->setMessages(instructions);
00114                 }
00115                 showingHelp = !showingHelp;
00116                 break;
00117             case SDLK_i:
00118                 log->record(1);
00119                 scene->capture();
00120                 break;
00121             case SDLK_q:
00122                 return false;
00123             case SDLK_p:
00124                 if (throbj){
00125                     if (throbj->isPausing()){
00126                         throbj->resume();
00127                     }else{
00128                         throbj->pause();
00129                     }
00130                 }
00131                 break;
00132             case SDLK_SPACE:
00133                 log->play(); 
00134                 break;
00135             case SDLK_t:
00136                 scene->toggleRobotState(); 
00137                 break;
00138             case SDLK_f:
00139                 log->faster();
00140                 break;
00141             case SDLK_s:
00142                 log->slower();
00143                 break;
00144             case SDLK_r:
00145                 log->record(20);
00146                 break;
00147             case SDLK_d:
00148             {
00149                 int drawMode = GLlink::drawMode()+1;
00150                 if (drawMode == GLlink::DM_NUM) drawMode=0;
00151                 GLlink::drawMode(drawMode);
00152                 break;
00153             }
00154             case SDLK_n:
00155                 scene->nextCamera();
00156                 break;
00157             case SDLK_o:
00158                 scene->nextObject();
00159                 break;
00160             case SDLK_c:
00161                 log->clear();
00162                 scene->clear();
00163                 break;
00164             case SDLK_g:
00165                 scene->showFloorGrid(!scene->showFloorGrid());
00166                 break;
00167             case SDLK_l:
00168                 scene->defaultLights(!scene->defaultLights());
00169                 break;
00170             case SDLK_v:
00171                 {
00172                     hrp::Vector3 center = scene->center();
00173                     xCenter = center[0];
00174                     yCenter = center[1];
00175                     zCenter = center[2];
00176                 }
00177                 break;
00178             case SDLK_RIGHT:
00179                 if (isControlPressed){
00180                     log->tail();
00181                 }else{
00182                     log->next(delta);
00183                 }
00184                 break;
00185             case SDLK_LEFT:
00186                 if (isControlPressed){
00187                     log->head();
00188                 }else{
00189                     log->prev(delta);
00190                 }
00191                 break;
00192             case SDLK_LSHIFT:
00193             case SDLK_RSHIFT:
00194                 isShiftPressed = true;
00195                 break;
00196             case SDLK_LCTRL:
00197             case SDLK_RCTRL:
00198                 isControlPressed = true;
00199                 break;
00200             }
00201             break;
00202         }
00203         case SDL_KEYUP:
00204             switch(event.key.keysym.sym){
00205             case SDLK_LSHIFT:
00206             case SDLK_RSHIFT:
00207                     isShiftPressed = false;
00208                     break;
00209             case SDLK_LCTRL:
00210             case SDLK_RCTRL:
00211                 isControlPressed = false;
00212                 break;
00213             }
00214             break;
00215             
00216         case SDL_MOUSEBUTTONDOWN:
00217             switch(event.button.button){
00218             case SDL_BUTTON_LEFT:
00219                 if (event.button.y > height-SLIDER_AREA_HEIGHT){
00220                     log->move(sliderRatio(event.button.x));
00221                     buttonPressedInSliderArea = true;
00222                 }else{
00223                     buttonPressedInSliderArea = false;
00224                 }
00225                 break;
00226             case SDL_BUTTON_MIDDLE:
00227                 break;
00228             case SDL_BUTTON_RIGHT:
00229                 break;
00230             case SDL_BUTTON_WHEELUP:
00231                 if (isShiftPressed){
00232                     xCenter -= 0.2*cos(tilt)*cos(pan);
00233                     yCenter -= 0.2*cos(tilt)*sin(pan);
00234                     zCenter -= 0.2*sin(tilt);
00235                 }else{
00236                     radius *= 0.9;
00237                     if (radius < 0.1) radius = 0.1; 
00238                 }
00239                 break;
00240             case SDL_BUTTON_WHEELDOWN:
00241                 if (isShiftPressed) {
00242                     xCenter += 0.2*cos(tilt)*cos(pan);
00243                     yCenter += 0.2*cos(tilt)*sin(pan);
00244                     zCenter += 0.2*sin(tilt);
00245                 }else{
00246                     radius *= 1.1;
00247                 }
00248                 break;
00249             }
00250         case SDL_MOUSEBUTTONUP:
00251             switch(event.button.button){
00252             case SDL_BUTTON_LEFT:
00253                 break;
00254             case SDL_BUTTON_MIDDLE:
00255                 break;
00256             case SDL_BUTTON_RIGHT:
00257                 break;
00258             case SDL_BUTTON_WHEELUP:
00259                 break;
00260             case SDL_BUTTON_WHEELDOWN:
00261                 break;
00262             }
00263             break;
00264         case SDL_MOUSEMOTION:
00265         {
00266             int dx = event.motion.xrel;
00267             int dy = event.motion.yrel;
00268             if (event.motion.state&SDL_BUTTON(SDL_BUTTON_LEFT)){
00269                 if (isShiftPressed){
00270                     radius *= (1+ 0.1*dy);
00271                     if (radius < 0.1) radius = 0.1; 
00272                 }else{
00273                     if (buttonPressedInSliderArea){
00274                         log->move(sliderRatio(event.motion.x));
00275                     }else{
00276                         pan  -= 0.05*dx;
00277                         tilt += 0.05*dy;
00278                         if (tilt >  M_PI/2) tilt =  M_PI/2;
00279                         if (tilt < -M_PI/2) tilt = -M_PI/2;
00280                     }
00281                 }
00282             }else if (event.motion.state&SDL_BUTTON(SDL_BUTTON_RIGHT)){
00283                 xCenter += sin(pan)*dx*0.01;
00284                 yCenter -= cos(pan)*dx*0.01;
00285                 zCenter += dy*0.01;
00286             }
00287             scene->showSlider(event.motion.y > height-SLIDER_AREA_HEIGHT);
00288         }
00289         break;
00290         case SDL_VIDEORESIZE:
00291             width = event.resize.w;
00292             height = event.resize.h;
00293             SDL_SetVideoMode(width,height,32,SDL_HWSURFACE | SDL_GL_DOUBLEBUFFER | SDL_OPENGL | SDL_RESIZABLE);
00294             scene->setScreenSize(width, height);
00295             break;
00296         }
00297     }
00298     return true;
00299 }
00300 
00301 static void drawString(const char *str)
00302 {
00303     for (unsigned int i=0; i<strlen(str); i++){
00304         glutBitmapCharacter(GLUT_BITMAP_9_BY_15, str[i]);
00305     }
00306 }
00307 
00308 void SDLwindow::draw()
00309 {
00310     if (scene->getDefaultCamera() == scene->getCamera()){
00311         hrp::BodyPtr target = scene->targetObject();
00312         double yaw = pan;
00313         double x,y,z;
00314         if (target){
00315             GLlink *root = (GLlink *)target->rootLink();
00316             root->getPosition(x,y,z);
00317             hrp::Matrix33 R;
00318             root->getRotation(R);
00319             hrp::Vector3 rpy = hrp::rpyFromRot(R);
00320             yaw += rpy[2];
00321         }else{
00322             x = xCenter; y = yCenter; z = zCenter;
00323         }
00324         double xEye = x + radius*cos(tilt)*cos(yaw);
00325         double yEye = y + radius*cos(tilt)*sin(yaw);
00326         double zEye = z + radius*sin(tilt);
00327         scene->getCamera()->setViewTarget(x,y,z);
00328         scene->getCamera()->setViewPoint(xEye, yEye, zEye);
00329     }
00330     scene->setView();
00331     
00332     glClear(GL_COLOR_BUFFER_BIT| GL_DEPTH_BUFFER_BIT);
00333 
00334     scene->draw();
00335 }
00336 
00337 void SDLwindow::swapBuffers()
00338 {
00339     SDL_GL_SwapBuffers();
00340 }
00341 
00342 bool SDLwindow::oneStep()
00343 {
00344     if (!initialized){
00345         // init() must be executed in the thread where draw() is called
00346         init();
00347     }
00348     double startT = SDL_GetTicks();
00349     if (!processEvents()) return false;
00350     draw();
00351     swapBuffers();
00352     double dt = SDL_GetTicks() - startT;
00353     if (dt < 1000.0/30){
00354         SDL_Delay(1000.0/30 - dt);
00355     }
00356     return true;
00357 }
00358 
00359 void SDLwindow::setView(double T[16])
00360 {
00361     pan = atan2(T[6], T[2]); // atan2(Rzy,Rzx)
00362     tilt = atan2(T[10], sqrt(T[2]*T[2]+T[6]*T[6]));
00363     radius = 5.0;
00364     xCenter = -radius * T[ 2] + T[ 3];
00365     yCenter = -radius * T[ 6] + T[ 7];
00366     zCenter = -radius * T[10] + T[11];
00367 }
00368 
00369 void SDLwindow::setSize(int w, int h)
00370 {
00371     width = w; 
00372     height = h;
00373 }


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed May 15 2019 05:02:19