SDLUtil.cpp
Go to the documentation of this file.
1 #include <math.h>
2 #ifdef __APPLE__
3 #include <OpenGL/glu.h>
4 #include <GLUT/glut.h>
5 #else
6 #include <GL/glu.h>
7 #include <GL/glut.h>
8 #endif
9 #include <SDL.h>
10 #include "hrpsys/util/ThreadedObject.h"
11 #include "hrpsys/util/LogManagerBase.h"
12 #include "hrpsys/util/GLsceneBase.h"
13 #include "hrpsys/util/GLcamera.h"
14 #include "hrpsys/util/GLlink.h"
15 #include "SDLUtil.h"
16 
17 
19  ThreadedObject* i_throbj) :
20  scene(i_scene), log(i_log), throbj(i_throbj),
21  width(640), height(480),
22  pan(M_PI/4), tilt(M_PI/16), radius(5),
23  isShiftPressed(false), isControlPressed(false),
24  xCenter(0), yCenter(0), zCenter(0.8),
25  showingHelp(false), initialized(false)
26 {
27  helpcommand.push_back("h: help");
28  instructions.push_back("q: quit");
29  instructions.push_back("SPACE: play/stop");
30  instructions.push_back("f: faster");
31  instructions.push_back("s: slower");
32  instructions.push_back("r: record movie");
33  instructions.push_back("t: toggle robot state");
34  instructions.push_back("d: rotate draw mode");
35  instructions.push_back("n: select next camera");
36  instructions.push_back("c: clear scene");
37  instructions.push_back("g: toggle floor grid");
38  instructions.push_back("l: toggle default lights");
39  instructions.push_back("o: rotate target object");
40  instructions.push_back("v: view center of objects");
41  if (throbj){
42  instructions.push_back("p: pause/resume background thread");
43  }
44  scene->setMessages(helpcommand);
45 }
46 
48 {
49  if ( initialized ) {
50  SDL_Quit();
51  }
52 }
53 
54 bool SDLwindow::init(int w, int h, bool resizable)
55 {
56  if (w) width = w;
57  if (h) height = h;
58 
59  int argc=1;
60  char *argv[] = {(char *)"dummy"};
61  glutInit(&argc, argv); // for bitmap fonts
62 
63  if(SDL_Init(SDL_INIT_VIDEO)<0) {
64  fprintf(stderr,"failed to initialize SDL.\n");
65  return false;
66  }
67 
68  SDL_GL_SetAttribute(SDL_GL_DOUBLEBUFFER,1);
69  SDL_GL_SetAttribute(SDL_GL_SWAP_CONTROL,1);
70  SDL_Surface *screen;
71  int flag = SDL_HWSURFACE | SDL_GL_DOUBLEBUFFER | SDL_OPENGL;
72  if (resizable) flag |= SDL_RESIZABLE;
73  screen=SDL_SetVideoMode(width,height,32,flag);
74  if(!screen) {
75  fprintf(stderr,"failed to set video mode to %dx%dx32.\n",width,height);
76  SDL_Quit();
77  return false;
78  }
79  SDL_WM_SetCaption("hrpsys viewer", NULL);
80  SDL_EnableKeyRepeat(SDL_DEFAULT_REPEAT_DELAY,SDL_DEFAULT_REPEAT_INTERVAL);
81 
82  scene->init();
83  scene->setScreenSize(width, height);
84 
85  initialized = true;
86  return true;
87 }
88 
89 double SDLwindow::sliderRatio(double x)
90 {
91  double ratio = (x - SLIDER_SIDE_MARGIN)/(width - SLIDER_SIDE_MARGIN*2);
92  if (ratio < 0.0) ratio = 0.0;
93  if (ratio > 1.0) ratio = 1.0;
94  return ratio;
95 }
96 
98 {
99  SDL_Event event;
100  while(SDL_PollEvent(&event)){
101  switch(event.type){
102  case SDL_QUIT:
103  return false;
104  case SDL_KEYDOWN:
105  {
106  //printf("%d\n", event.key.keysym.sym);
107  int delta = isShiftPressed ? 10 : 1;
108  switch(event.key.keysym.sym){
109  case SDLK_h:
110  if (showingHelp){
111  scene->setMessages(helpcommand);
112  }else{
113  scene->setMessages(instructions);
114  }
116  break;
117  case SDLK_i:
118  log->record(1);
119  scene->capture();
120  break;
121  case SDLK_q:
122  return false;
123  case SDLK_p:
124  if (throbj){
125  if (throbj->isPausing()){
126  throbj->resume();
127  }else{
128  throbj->pause();
129  }
130  }
131  break;
132  case SDLK_SPACE:
133  log->play();
134  break;
135  case SDLK_t:
136  scene->toggleRobotState();
137  break;
138  case SDLK_f:
139  log->faster();
140  break;
141  case SDLK_s:
142  log->slower();
143  break;
144  case SDLK_r:
145  log->record(20);
146  break;
147  case SDLK_d:
148  {
149  int drawMode = GLlink::drawMode()+1;
150  if (drawMode == GLlink::DM_NUM) drawMode=0;
151  GLlink::drawMode(drawMode);
152  break;
153  }
154  case SDLK_n:
155  scene->nextCamera();
156  break;
157  case SDLK_o:
158  scene->nextObject();
159  break;
160  case SDLK_c:
161  log->clear();
162  scene->clear();
163  break;
164  case SDLK_g:
165  scene->showFloorGrid(!scene->showFloorGrid());
166  break;
167  case SDLK_l:
168  scene->defaultLights(!scene->defaultLights());
169  break;
170  case SDLK_v:
171  {
172  hrp::Vector3 center = scene->center();
173  xCenter = center[0];
174  yCenter = center[1];
175  zCenter = center[2];
176  }
177  break;
178  case SDLK_RIGHT:
179  if (isControlPressed){
180  log->tail();
181  }else{
182  log->next(delta);
183  }
184  break;
185  case SDLK_LEFT:
186  if (isControlPressed){
187  log->head();
188  }else{
189  log->prev(delta);
190  }
191  break;
192  case SDLK_LSHIFT:
193  case SDLK_RSHIFT:
194  isShiftPressed = true;
195  break;
196  case SDLK_LCTRL:
197  case SDLK_RCTRL:
198  isControlPressed = true;
199  break;
200  }
201  break;
202  }
203  case SDL_KEYUP:
204  switch(event.key.keysym.sym){
205  case SDLK_LSHIFT:
206  case SDLK_RSHIFT:
207  isShiftPressed = false;
208  break;
209  case SDLK_LCTRL:
210  case SDLK_RCTRL:
211  isControlPressed = false;
212  break;
213  }
214  break;
215 
216  case SDL_MOUSEBUTTONDOWN:
217  switch(event.button.button){
218  case SDL_BUTTON_LEFT:
219  if (event.button.y > height-SLIDER_AREA_HEIGHT){
220  log->move(sliderRatio(event.button.x));
222  }else{
224  }
225  break;
226  case SDL_BUTTON_MIDDLE:
227  break;
228  case SDL_BUTTON_RIGHT:
229  break;
230  case SDL_BUTTON_WHEELUP:
231  if (isShiftPressed){
232  xCenter -= 0.2*cos(tilt)*cos(pan);
233  yCenter -= 0.2*cos(tilt)*sin(pan);
234  zCenter -= 0.2*sin(tilt);
235  }else{
236  radius *= 0.9;
237  if (radius < 0.1) radius = 0.1;
238  }
239  break;
240  case SDL_BUTTON_WHEELDOWN:
241  if (isShiftPressed) {
242  xCenter += 0.2*cos(tilt)*cos(pan);
243  yCenter += 0.2*cos(tilt)*sin(pan);
244  zCenter += 0.2*sin(tilt);
245  }else{
246  radius *= 1.1;
247  }
248  break;
249  }
250  case SDL_MOUSEBUTTONUP:
251  switch(event.button.button){
252  case SDL_BUTTON_LEFT:
253  break;
254  case SDL_BUTTON_MIDDLE:
255  break;
256  case SDL_BUTTON_RIGHT:
257  break;
258  case SDL_BUTTON_WHEELUP:
259  break;
260  case SDL_BUTTON_WHEELDOWN:
261  break;
262  }
263  break;
264  case SDL_MOUSEMOTION:
265  {
266  int dx = event.motion.xrel;
267  int dy = event.motion.yrel;
268  if (event.motion.state&SDL_BUTTON(SDL_BUTTON_LEFT)){
269  if (isShiftPressed){
270  radius *= (1+ 0.1*dy);
271  if (radius < 0.1) radius = 0.1;
272  }else{
274  log->move(sliderRatio(event.motion.x));
275  }else{
276  pan -= 0.05*dx;
277  tilt += 0.05*dy;
278  if (tilt > M_PI/2) tilt = M_PI/2;
279  if (tilt < -M_PI/2) tilt = -M_PI/2;
280  }
281  }
282  }else if (event.motion.state&SDL_BUTTON(SDL_BUTTON_RIGHT)){
283  xCenter += sin(pan)*dx*0.01;
284  yCenter -= cos(pan)*dx*0.01;
285  zCenter += dy*0.01;
286  }
287  scene->showSlider(event.motion.y > height-SLIDER_AREA_HEIGHT);
288  }
289  break;
290  case SDL_VIDEORESIZE:
291  width = event.resize.w;
292  height = event.resize.h;
293  SDL_SetVideoMode(width,height,32,SDL_HWSURFACE | SDL_GL_DOUBLEBUFFER | SDL_OPENGL | SDL_RESIZABLE);
294  scene->setScreenSize(width, height);
295  break;
296  }
297  }
298  return true;
299 }
300 
301 static void drawString(const char *str)
302 {
303  for (unsigned int i=0; i<strlen(str); i++){
304  glutBitmapCharacter(GLUT_BITMAP_9_BY_15, str[i]);
305  }
306 }
307 
309 {
310  if (scene->getDefaultCamera() == scene->getCamera()){
311  hrp::BodyPtr target = scene->targetObject();
312  double yaw = pan;
313  double x,y,z;
314  if (target){
315  GLlink *root = (GLlink *)target->rootLink();
316  root->getPosition(x,y,z);
318  root->getRotation(R);
319  hrp::Vector3 rpy = hrp::rpyFromRot(R);
320  yaw += rpy[2];
321  }else{
322  x = xCenter; y = yCenter; z = zCenter;
323  }
324  double xEye = x + radius*cos(tilt)*cos(yaw);
325  double yEye = y + radius*cos(tilt)*sin(yaw);
326  double zEye = z + radius*sin(tilt);
327  scene->getCamera()->setViewTarget(x,y,z);
328  scene->getCamera()->setViewPoint(xEye, yEye, zEye);
329  }
330  scene->setView();
331 
332  glClear(GL_COLOR_BUFFER_BIT| GL_DEPTH_BUFFER_BIT);
333 
334  scene->draw();
335 }
336 
338 {
339  SDL_GL_SwapBuffers();
340 }
341 
343 {
344  if (!initialized){
345  // init() must be executed in the thread where draw() is called
346  init();
347  }
348  double startT = SDL_GetTicks();
349  if (!processEvents()) return false;
350  draw();
351  swapBuffers();
352  double dt = SDL_GetTicks() - startT;
353  if (dt < 1000.0/30){
354  SDL_Delay(1000.0/30 - dt);
355  }
356  return true;
357 }
358 
359 void SDLwindow::setView(double T[16])
360 {
361  pan = atan2(T[6], T[2]); // atan2(Rzy,Rzx)
362  tilt = atan2(T[10], sqrt(T[2]*T[2]+T[6]*T[6]));
363  radius = 5.0;
364  xCenter = -radius * T[ 2] + T[ 3];
365  yCenter = -radius * T[ 6] + T[ 7];
366  zCenter = -radius * T[10] + T[11];
367 }
368 
369 void SDLwindow::setSize(int w, int h)
370 {
371  width = w;
372  height = h;
373 }
bool showingHelp
Definition: SDLUtil.h:29
void draw()
Definition: SDLUtil.cpp:308
#define SLIDER_SIDE_MARGIN
Definition: GLsceneBase.h:28
void setSize(int w, int h)
Definition: SDLUtil.cpp:369
LogManagerBase * log
Definition: SDLUtil.h:23
virtual bool record(double i_fps)=0
double tilt
Definition: SDLUtil.h:26
virtual void slower()=0
~SDLwindow()
Definition: SDLUtil.cpp:47
SDLwindow(GLsceneBase *i_scene, LogManagerBase *i_lm, ThreadedObject *i_throbj=NULL)
Definition: SDLUtil.cpp:18
double pan
Definition: SDLUtil.h:26
bool processEvents()
Definition: SDLUtil.cpp:97
double radius
Definition: SDLUtil.h:26
bool init(int w=0, int h=0, bool resizable=true)
Definition: SDLUtil.cpp:54
png_uint_32 i
hrp::Matrix33 getRotation()
ThreadedObject * throbj
Definition: SDLUtil.h:24
virtual void faster()=0
void setView(double T[16])
Definition: SDLUtil.cpp:359
png_infop png_uint_32 * width
bool initialized
Definition: SDLUtil.h:31
Eigen::Vector3d Vector3
hrp::Vector3 getPosition()
bool oneStep()
Definition: SDLUtil.cpp:342
Eigen::Matrix3d Matrix33
png_infop png_uint_32 png_uint_32 * height
virtual void move(double ratio)=0
bool isControlPressed
Definition: SDLUtil.h:27
virtual void next(int delta)=0
double sliderRatio(double x)
Definition: SDLUtil.cpp:89
bool isShiftPressed
Definition: SDLUtil.h:27
bool buttonPressedInSliderArea
Definition: SDLUtil.h:29
static void drawString(const char *str)
Definition: SDLUtil.cpp:301
HRP_UTIL_EXPORT Vector3 rpyFromRot(const Matrix33 &m)
#define M_PI
virtual void head()=0
virtual void tail()=0
std::vector< std::string > instructions
Definition: SDLUtil.h:30
int height
Definition: SDLUtil.h:25
virtual void clear()=0
double yCenter
Definition: SDLUtil.h:28
virtual void prev(int delta)=0
void swapBuffers()
Definition: SDLUtil.cpp:337
double xCenter
Definition: SDLUtil.h:28
virtual void play()=0
int width
Definition: SDLUtil.h:25
#define SLIDER_AREA_HEIGHT
Definition: GLsceneBase.h:27
png_infop png_uint_32 flag
std::vector< std::string > helpcommand
Definition: SDLUtil.h:30
double zCenter
Definition: SDLUtil.h:28


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