/*  slam3 (c) 2006 Kris Beevers  This file is part of slam3.  slam3 is free software; you can redistribute it and/or modify it  under the terms of the GNU General Public License as published by  the Free Software Foundation; either version 2 of the License, or  (at your option) any later version.  slam3 is distributed in the hope that it will be useful, but WITHOUT  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY  or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public  License for more details.  You should have received a copy of the GNU General Public License  along with slam3; if not, write to the Free Software Foundation,  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA*/#include "slam.hpp"#include "options.hpp"#include <GL/glut.h>#include <algorithm>extern void process();// in slam.cppextern pose_history_t gps_hist;extern bool done;// FIXME come up with a better way to pass this stuff in#include "rbpf.hpp"#ifdef GRIDSLAM // FIXME deal with these in slam_interface somehowtypedef slam_scan_match mapper_type;//typedef slam_scan_match_2 mapper_type;#elsetypedef slam_interface mapper_type;#endifextern mapper_type mapper;namespace gui {const char *title_prefix = "SLAM2 - ";int mouse_x, mouse_y;bool paused = false, movie = false, mouse_left = false, mouse_right = false;uint32_t redraw_freq = 1;int viewport_w, viewport_h, viewport_w_orig, viewport_h_orig;double workspace_w, workspace_h, scale = 1, trans_x, trans_y, extra_x, extra_y;uint32_t ss_count = 0;char ss_fn[256];bool screenshot(const char *prefix){  sprintf(ss_fn, "%s%08d.ppm", prefix, ss_count);  ++ss_count;  unsigned int size = 3 * viewport_w * viewport_h;  unsigned char *buffer = new unsigned char[size];  memset(buffer, 0, size);  glPixelStorei(GL_PACK_ALIGNMENT, 1);  glReadPixels(0, 0, viewport_w, viewport_h, GL_RGB, GL_UNSIGNED_BYTE, buffer);  FILE *out = fopen(ss_fn, "wb");  if(!out)    return false;  fprintf(out, "P6\n%d\n%d\n255\n", viewport_w, viewport_h);  const unsigned char *pix;  int y, x;  // this will flip the y-axis  for(y = viewport_h - 1; y >= 0; --y) {    for(x = 0; x < int(viewport_h); ++x) {      pix = &buffer[3*(y*viewport_w+x)];      fwrite(pix, 3, 1, out);    }  }  fclose(out);  return true;}void idle(){  if(!paused) {    for(uint32_t i = 0; i < redraw_freq; ++i)      process();    glutPostRedisplay();  } else    usleep(100000);}void display(){  glClearColor(0.7, 0.7, 0.7, 1);  glClear(GL_COLOR_BUFFER_BIT);  glMatrixMode(GL_PROJECTION);  glLoadIdentity();  gluOrtho2D(scale * (-workspace_w / 2 + trans_x),	     scale * (workspace_w / 2 + trans_x + extra_x),	     scale * (-workspace_h / 2 + trans_y),	     scale * (workspace_h / 2 + trans_y + extra_y));  glMatrixMode(GL_MODELVIEW);  glLoadIdentity();  // because occupancy grid data is stored at a different orientation  glRotatef(-90, 0, 0, 1);  // axes  glLineWidth(1);  glColor3f(0.8,0.8,0.8);  glBegin(GL_LINES);  glVertex2d(-100000, 0); glVertex2d(100000, 0);  glVertex2d(0, -100000); glVertex2d(0, 100000);  glEnd();  glColor3f(0.0, 0.25, 0.25);  gps_hist.render(); // gps trajectory  mapper.render();  glutSwapBuffers();  if(movie)    screenshot("movie/");}void reshape(int width, int height){  glViewport(0, 0, width, height);  viewport_w = width;  viewport_h = height;  extra_x = workspace_w * (float(width) - viewport_w_orig) / float(viewport_w_orig);  extra_y = workspace_h * (float(height) - viewport_h_orig) / float(viewport_h_orig);}void keyboard(uint8_t key, int x, int y){  switch(key) {  case 'Q': // quit  case 'q':    printf("\ngoodbye\n");    if(!done)      mapper.write_to_file("output");    exit(0);  case 'S': // screenshot  case 's':    screenshot("");    printf("wrote %s\n", ss_fn);    break;  case 'M': // start/stop dump every screen (for movie)  case 'm':    movie = !movie;    printf(movie ? "dumping every frame to movie/...\n" : "stopped dumping frames\n");    break;  case 'N': // process one step if paused  case 'n':    if(paused) {      printf("processing one step, press SPACE for full throttle\n");      process();    }    break;  case ' ': // pause    paused = !paused;    printf(paused ? "\npaused, press SPACE to continue\n" : "unpaused\n");    return; // no redisplay  }  glutPostRedisplay();}void mouse(int button, int state, int x, int y){  mouse_x = x;  mouse_y = y;  if(state == GLUT_UP) {    if(button == GLUT_LEFT_BUTTON) mouse_left = false;    else if(button == GLUT_RIGHT_BUTTON) mouse_right = false;  } else {    if(button == GLUT_LEFT_BUTTON) mouse_left = true;    else if(button == GLUT_RIGHT_BUTTON) mouse_right = true;  }}void motion(int x, int y){  int dx = x - mouse_x, dy = y - mouse_y;  if(dx == 0 && dy == 0)    return;  if(mouse_left) { // pan    trans_x -= workspace_w * float(dx) / float(viewport_w_orig);    trans_y += workspace_h * float(dy) / float(viewport_h_orig);  } else if(mouse_right) // zoom    scale *= exp(0.05 * double(dy) + 0.01);  mouse_x = x;  mouse_y = y;  if(mouse_left || mouse_right)    glutPostRedisplay();}void special(int key, int x, int y){  if(key == GLUT_KEY_HOME) {    scale = 1.0;    trans_x = trans_y = 0.0;  }  glutPostRedisplay();}void run(int argc, char **argv, const char *filename){  // get commandline options that we use for rendering  viewport_w = viewport_w_orig = std::max(options::quickget<int>("gui-width"), 1);  viewport_h = viewport_h_orig = std::max(options::quickget<int>("gui-height"), 1);  workspace_w = std::max(options::quickget<double>("workspace-width"), 0.1);  workspace_h = std::max(options::quickget<double>("workspace-height"), 0.1);  redraw_freq = std::max(options::quickget<uint32_t>("redraw-freq"), 1u);  movie = options::quickget<bool>("movie");  // set up renderer  glutInit(&argc, argv);  glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);  glutInitWindowSize(viewport_w, viewport_h);  char *title = new char[strlen(title_prefix) + strlen(filename) + 1];  sprintf(title, "%s%s", title_prefix, filename);  glutCreateWindow(title);  glutDisplayFunc(display);  glutReshapeFunc(reshape);  glutKeyboardFunc(keyboard);  glutMouseFunc(mouse);  glutMotionFunc(motion);  glutSpecialFunc(special);  glutIdleFunc(idle);  glutMainLoop();}}; // namespace gui