canvas.cc

Go to the documentation of this file.

#include "stage.hh"
#include "canvas.hh"
#include "worldfile.hh"
#include "texture_manager.hh"
#include "replace.h"

#include <string>
#include <sstream>
#include <png.h>
#include <algorithm>
#include <functional>      // For greater<int>( )

#include "region.hh"
#include "file_manager.hh"
#include "options_dlg.hh"

using namespace Stg;

static  const int checkImageWidth = 2;
static  const int       checkImageHeight = 2;
static  GLubyte checkImage[checkImageHeight][checkImageWidth][4];
static bool blur = true;

static bool init_done = false;
static bool texture_load_done = false;

//GLuint glowTex;
GLuint checkTex;

void Canvas::TimerCallback( Canvas* c )
{
  if( c->world->dirty )
    {
      //puts( "timer redraw" );
      c->redraw();
      c->world->dirty = false;
    }
  
  Fl::repeat_timeout( c->interval/1000.0,
                      (Fl_Timeout_Handler)Canvas::TimerCallback, 
                      c);
}

Canvas::Canvas( WorldGui* world, 
                int x, int y, 
                int width, int height) :
  Fl_Gl_Window( x, y, width, height ),
  colorstack(),  
  models_sorted(),
  current_camera( NULL ),
  camera(),
  perspective_camera(),
  dirty_buffer( false ),  
  wf( NULL ),
  startx( -1 ),
  starty( -1 ),
  selected_models(),
  last_selection( NULL ),
  interval( 40 ), // msec between redraws
  // initialize Option objects
  //  showBlinken( "Blinkenlights", "show_blinkenlights", "", true, world ), 
  showBBoxes( "Debug/Bounding boxes", "show_boundingboxes", "^b", false, world ),
  showBlocks( "Blocks", "show_blocks", "b", true, world ),
  showBlur( "Trails/Blur", "show_trailblur", "^d", false, world ),
  showClock( "Clock", "show_clock", "c", true, world ),
  showData( "Data", "show_data", "d", false, world ),
  showFlags( "Flags", "show_flags", "l",  true, world ),
  showFollow( "Follow", "show_follow", "f", false, world ),
  showFootprints( "Footprints", "show_footprints", "o", false, world ),
  showGrid( "Grid", "show_grid", "g", true, world ),
  showOccupancy( "Debug/Occupancy", "show_occupancy", "^o", false, world ),
  showScreenshots( "Save screenshots", "screenshots", "", false, world ),
  showStatus( "Status", "show_status", "s", true, world ),
  showTrailArrows( "Trails/Rising Arrows", "show_trailarrows", "^a", false, world ),
  showTrailRise( "Trails/Rising blocks", "show_trailrise", "^r", false, world ),
  showTrails( "Trails/Fast", "show_trailfast", "^f", false, world ),
  showVoxels( "Debug/Voxels", "show_voxels", "^v", false, world ),
  pCamOn( "Perspective camera", "pcam_on", "r", false, world ),
  visualizeAll( "Selected only", "vis_all", "v", false, world ),
  // and the rest 
  graphics( true ),
  world( world ),
  frames_rendered_count( 0 ),
  screenshot_frame_skip( 1 )
{
  end();  
  //show(); // must do this so that the GL context is created before configuring GL
  // but that line causes a segfault in Linux/X11! TODO: test in OS X
  
  perspective_camera.setPose( 0.0, -4.0, 3.0 );
  current_camera = &camera;
  setDirtyBuffer();
        
  // enable accumulation buffer
  //mode( mode() | FL_ACCUM );
  //assert( can_do( FL_ACCUM ) );
}

void Canvas::InitGl()
{
  valid(1);
  FixViewport(w(), h());
  
  // set gl state that won't change every redraw
  glClearColor ( 0.7, 0.7, 0.8, 1.0);
  glDisable( GL_LIGHTING );
  glEnable( GL_DEPTH_TEST );
  glDepthFunc( GL_LESS );
  // culling disables text drawing on OS X, so I've disabled it until I understand why
  //glCullFace( GL_BACK );
  //glEnable (GL_CULL_FACE);
  glEnable( GL_BLEND );
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA );
  glEnable( GL_LINE_SMOOTH );
  glHint( GL_LINE_SMOOTH_HINT, GL_FASTEST );
  glDepthMask( GL_TRUE );
  glEnable( GL_TEXTURE_2D );
  glEnableClientState( GL_VERTEX_ARRAY );
  glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
  
  // install a font
  gl_font( FL_HELVETICA, 12 );  

  blur = false;
  
  glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE);
    
  init_done = true; 
}


void Canvas::InitTextures()
{
  // load textures
  std::string fullpath = FileManager::findFile( "assets/stall.png" );
  if ( fullpath == "" ) 
    {
      PRINT_DEBUG( "Unable to load stall texture.\n" );
    }
 
  //printf( "stall icon %s\n", fullpath.c_str() );
 
  GLuint stall_id = TextureManager::getInstance().loadTexture( fullpath.c_str() );
  TextureManager::getInstance()._stall_texture_id = stall_id;

  fullpath = FileManager::findFile( "assets/mainspower.png" );
  if ( fullpath == "" ) 
    {
      PRINT_DEBUG( "Unable to load mains texture.\n" );
    }
  
  //    printf( "mains icon %s\n", fullpath.c_str() );

  GLuint mains_id = TextureManager::getInstance().loadTexture( fullpath.c_str() );
  TextureManager::getInstance()._mains_texture_id = mains_id;
  
  //   // generate a small glow texture
  //   GLubyte* pixels = new GLubyte[ 4 * 128 * 128 ];

  //   for( int x=0; x<128; x++ )
  //     for( int y=0; y<128; y++ )
  //            {                 
  //              GLubyte* p = &pixels[ 4 * (128*y + x)];
  //              p[0] = (GLubyte)255; // red
  //              p[1] = (GLubyte)0; // green
  //              p[2] = (GLubyte)0; // blue
  //              p[3] = (GLubyte)128; // alpha
  //            }


  //   glGenTextures(1, &glowTex );
  //   glBindTexture( GL_TEXTURE_2D, glowTex );
  
  //   glTexImage2D( GL_TEXTURE_2D, 0, GL_RGBA, 128, 128, 0, 
  //                                     GL_RGBA, GL_UNSIGNED_BYTE, pixels );

  //   delete[] pixels;

  // draw a check into a bitmap, then load that into a texture
  int i, j;
  for (i = 0; i < checkImageHeight; i++) 
    for (j = 0; j < checkImageWidth; j++) 
      {                 
        int even = (i+j)%2;
        checkImage[i][j][0] = (GLubyte) 255 - 10*even;
        checkImage[i][j][1] = (GLubyte) 255 - 10*even;
        checkImage[i][j][2] = (GLubyte) 255;// - 5*even;
        checkImage[i][j][3] = 255;
      }
  
  glGenTextures(1, &checkTex );          
  glBindTexture(GL_TEXTURE_2D, checkTex);
  
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
  
  glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, checkImageWidth, checkImageHeight, 
               0, GL_RGBA, GL_UNSIGNED_BYTE, checkImage);

  texture_load_done = true;
}


Canvas::~Canvas()
{ 
  // nothing to do
}

Model* Canvas::getModel( int x, int y )
{
  // render all models in a unique color
  make_current(); // make sure the GL context is current
  glClearColor( 1,1,1,1 ); // white
  glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
  glLoadIdentity();
  current_camera->SetProjection();
  current_camera->Draw();

  glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
  glDisable(GL_DITHER);
  glDisable(GL_BLEND); // turns off alpha blending, so we read back
  // exactly what we write to a pixel

  // render all top-level, draggable models in a color that is their
  // id 
  FOR_EACH( it, world->World::children )
    {
      Model* mod = (*it);
                
      if( mod->gui.move )
        {
          uint8_t rByte, gByte, bByte, aByte;
          uint32_t modelId = mod->id;
          rByte = modelId;
          gByte = modelId >> 8;
          bByte = modelId >> 16;
          aByte = modelId >> 24;

          //printf("mod->Id(): 0x%X, rByte: 0x%X, gByte: 0x%X, bByte: 0x%X, aByte: 0x%X\n", modelId, rByte, gByte, bByte, aByte);
                        
          glColor4ub( rByte, gByte, bByte, aByte );
          mod->DrawPicker();
        }
    }
        
  glFlush(); // make sure the drawing is done

  // read the color of the pixel in the back buffer under the mouse
  // pointer
  GLint viewport[4];
  glGetIntegerv(GL_VIEWPORT,viewport);

  uint8_t rgbaByte[4];
  uint32_t modelId;
        
  glReadPixels( x,viewport[3]-y,1,1,
                GL_RGBA,GL_UNSIGNED_BYTE,&rgbaByte[0] );
        
  modelId = rgbaByte[0];
  modelId |= rgbaByte[1] << 8;
  modelId |= rgbaByte[2] << 16;
  //modelId |= rgbaByte[3] << 24;
        
  //    printf("Clicked rByte: 0x%X, gByte: 0x%X, bByte: 0x%X, aByte: 0x%X\n", rgbaByte[0], rgbaByte[1], rgbaByte[2], rgbaByte[3]);
  //    printf("-->model Id = 0x%X\n", modelId);
        
  Model* mod = Model::LookupId( modelId );

  //printf("%p %s %d %x\n", mod, mod ? mod->Token() : "(none)", modelId, modelId );

  // put things back the way we found them
  glEnable(GL_DITHER);
  glEnable(GL_BLEND);
  glClearColor ( 0.7, 0.7, 0.8, 1.0);
  
  // useful for debugging the picker
  //Screenshot();
        
  return mod;
}

bool Canvas::selected( Model* mod ) 
{
  return( std::find( selected_models.begin(), selected_models.end(), mod ) != selected_models.end() );
}

void Canvas::select( Model* mod ) {
  if( mod )
    {
      last_selection = mod;
      selected_models.push_front( mod );
                
      //                mod->Disable();
      redraw();
    }
}

void Canvas::unSelect( Model* mod ) 
{
  if( mod )
    {
      EraseAll( mod, selected_models );
      redraw();
    }
}

void Canvas::unSelectAll() 
{ 
  selected_models.clear();
}

// convert from 2d window pixel to 3d world coordinates
void Canvas::CanvasToWorld( int px, int py, 
                            double *wx, double *wy, double* wz )
{
  if( px <= 0 )
    px = 1;
  else if( px >= w() )
    px = w() - 1;
  if( py <= 0 )
    py = 1;
  else if( py >= h() )
    py = h() - 1;
        
  //redraw the screen only if the camera model isn't active.
  //TODO new selection technique will simply use drawfloor to result in z = 0 always and prevent strange behaviours near walls
  //TODO refactor, so glReadPixels reads (then caches) the whole screen only when the camera changes.
  if( true || dirtyBuffer() ) {
    glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
    current_camera->SetProjection();
    current_camera->Draw();
    DrawFloor(); //call this rather than renderFrame for speed - this won't give correct z values
    dirty_buffer = false;
  }
        
  int viewport[4];
  glGetIntegerv(GL_VIEWPORT, viewport);

  GLdouble modelview[16];
  glGetDoublev(GL_MODELVIEW_MATRIX, modelview);

  GLdouble projection[16];      
  glGetDoublev(GL_PROJECTION_MATRIX, projection);

  GLfloat pz;
  glReadPixels( px, h()-py, 1, 1, GL_DEPTH_COMPONENT, GL_FLOAT, &pz );
  gluUnProject( px, w()-py, pz, modelview, projection, viewport, wx,wy,wz );    
}

int Canvas::handle(int event) 
{
  //printf( "cam %.2f %.2f\n", camera.yaw(), camera.pitch() );

  switch(event) 
    {
    case FL_MOUSEWHEEL:
      if( pCamOn == true ) {
        perspective_camera.scroll( Fl::event_dy() / 10.0 );
      }
      else {
        camera.scale( Fl::event_dy(),  Fl::event_x(), w(), Fl::event_y(), h() );
      }
      invalidate();
      redraw();
      return 1;
                
    case FL_MOVE: // moused moved while no button was pressed
      if( Fl::event_state( FL_META ) )
        {
          puts( "TODO: HANDLE HISTORY" );
          //world->paused = ! world->paused;
          return 1;
        }
                
      if ( startx >=0 ) 
        {
          // mouse pointing to valid value
                         
          if( Fl::event_state( FL_CTRL ) ) // rotate the camera view
            {
              int dx = Fl::event_x() - startx;
              int dy = Fl::event_y() - starty;
                                  
              if( pCamOn == true ) {
                perspective_camera.addYaw( -dx );
                perspective_camera.addPitch( -dy );
              } 
              else {
                camera.addPitch( - 0.5 * static_cast<double>( dy ) );
                camera.addYaw( - 0.5 * static_cast<double>( dx ) );
              }
              invalidate();
              redraw();
            }
          else if( Fl::event_state( FL_ALT ) )
            {   
              int dx = Fl::event_x() - startx;
              int dy = Fl::event_y() - starty;
                                  
              if( pCamOn == true ) {
                perspective_camera.move( -dx, dy, 0.0 );
              } 
              else {
                camera.move( -dx, dy );
              }
              invalidate();
            }
        }
      startx = Fl::event_x();
      starty = Fl::event_y();
      return 1;
    case FL_PUSH: // button pressed
      {
        //else
        {
          Model* mod = getModel( startx, starty );
          startx = Fl::event_x();
          starty = Fl::event_y();
          selectedModel = false;
          switch( Fl::event_button() )
            {
            case 1:
              clicked_empty_space = ( mod == NULL );
              empty_space_startx = startx;
              empty_space_starty = starty;
              if( mod ) { 
                // clicked a model
                if ( Fl::event_state( FL_SHIFT ) ) {
                  // holding shift, toggle selection
                  if ( selected( mod ) ) 
                    unSelect( mod );
                  else {
                    select( mod );
                    selectedModel = true; // selected a model
                  }
                }
                else {
                  if ( !selected( mod ) ) {
                    // clicked on an unselected model while
                    //  not holding shift, this is the new
                    //  selection
                    unSelectAll();
                    select( mod );
                  }
                  selectedModel = true; // selected a model
                }
              }
                                  
              redraw(); // probably required                                     
              return 1;
            case 3:
              {
                // leave selections alone
                // rotating handled within FL_DRAG
                return 1;
              }
            default:
              return 0;
            }    
        }
      }
          
    case FL_DRAG: // mouse moved while button was pressed
      {
        int dx = Fl::event_x() - startx;
        int dy = Fl::event_y() - starty;

        if ( Fl::event_state( FL_BUTTON1 ) && Fl::event_state( FL_CTRL ) == false ) {
          // Left mouse button drag
          if ( selectedModel ) {
            // started dragging on a selected model
                                
            double sx,sy,sz;
            CanvasToWorld( startx, starty,
                           &sx, &sy, &sz );
            double x,y,z;
            CanvasToWorld( Fl::event_x(), Fl::event_y(),
                           &x, &y, &z );
            // move all selected models to the mouse pointer
            FOR_EACH( it, selected_models )
              {
                Model* mod = *it;
                mod->AddToPose( x-sx, y-sy, 0, 0 );
              }
          }
          else {
            // started dragging on empty space or an
            //  unselected model, move the canvas
            if( pCamOn == true ) {
              perspective_camera.move( -dx, dy, 0.0 );
            } 
            else {
              camera.move( -dx, dy );
            }
            invalidate(); // so the projection gets updated
          }
        }
        else if ( Fl::event_state( FL_BUTTON3 ) || ( Fl::event_state( FL_BUTTON1 ) &&  Fl::event_state( FL_CTRL )  ) ) {
          // rotate all selected models

          if( selected_models.size() )
            {
              FOR_EACH( it, selected_models )
                {
                  Model* mod = *it;
                  mod->AddToPose( 0,0,0, 0.05*(dx+dy) );
                }
            }
            else
              {
                //printf( "button 2\n" );
                
                int dx = Fl::event_x() - startx;
                int dy = Fl::event_y() - starty;
                
                if( pCamOn == true ) {
                  perspective_camera.addYaw( -dx );
                  perspective_camera.addPitch( -dy );
                } 
                else {
                  camera.addPitch( - 0.5 * static_cast<double>( dy ) );
                  camera.addYaw( - 0.5 * static_cast<double>( dx ) );
                }
              }
            invalidate();
            redraw();
            }             
          
          startx = Fl::event_x();
          starty = Fl::event_y();
          
          redraw();
          return 1;
        } // end case FL_DRAG
        
    case FL_RELEASE:   // mouse button released
      if( empty_space_startx == Fl::event_x() && empty_space_starty == Fl::event_y() && clicked_empty_space == true ) {
        // clicked on empty space, unselect all
        unSelectAll();
        redraw();
      }
      return 1;

    case FL_FOCUS:
    case FL_UNFOCUS:
      //.... Return 1 if you want keyboard events, 0 otherwise
      return 1;

    case FL_KEYBOARD:
      switch( Fl::event_key() )
        {
        case FL_Left:
          if( pCamOn == false ) { camera.move( -10, 0 ); } 
          else { perspective_camera.strafe( -0.5 ); } break;
        case FL_Right: 
          if( pCamOn == false ) {camera.move( 10, 0 ); } 
          else { perspective_camera.strafe( 0.5 ); } break;
        case FL_Down:  
          if( pCamOn == false ) {camera.move( 0, -10 ); } 
          else { perspective_camera.forward( -0.5 ); } break;
        case FL_Up:  
          if( pCamOn == false ) {camera.move( 0, 10 ); } 
          else { perspective_camera.forward( 0.5 ); } break;
        default:
          redraw(); // we probably set a display config - so need this
          return 0; // keypress unhandled
        }
                
      invalidate(); // update projection
      return 1;
                        
      //        case FL_SHORTCUT:
      //                //... shortcut, key is in Fl::event_key(), ascii in Fl::event_text()
      //                //... Return 1 if you understand/use the shortcut event, 0 otherwise...
      //                return 1;
    default:
      // pass other events to the base class...
      //printf( "EVENT %d\n", event );
      return Fl_Gl_Window::handle(event);
                        
    } // end switch( event )

  return 0;
}

void Canvas::FixViewport(int W,int H) 
{
  glLoadIdentity();
  glViewport(0,0,W,H);
}

void Canvas::AddModel( Model*  mod  )
{
  models_sorted.push_back( mod );
  redraw();
}

void Canvas::RemoveModel( Model*  mod  )
{
  printf( "removing model %s from canvas list\n", mod->Token() );
  EraseAll( mod, models_sorted );
}

void Canvas::DrawGlobalGrid()
{
  bounds3d_t bounds = world->GetExtent();

  glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );

  glEnable(GL_POLYGON_OFFSET_FILL);
  glPolygonOffset(2.0, 2.0);
  glDisable(GL_BLEND);

  glEnable(GL_TEXTURE_2D);
  glBindTexture(GL_TEXTURE_2D, checkTex );
  glColor3f( 1.0, 1.0, 1.0 );

  glBegin(GL_QUADS);
  glTexCoord2f( bounds.x.min/2.0, bounds.y.min/2.0 ); 
  glVertex2f( bounds.x.min, bounds.y.min );
  glTexCoord2f( bounds.x.max/2.0, bounds.y.min/2.0); 
  glVertex2f(  bounds.x.max, bounds.y.min );
  glTexCoord2f( bounds.x.max/2.0, bounds.y.max/2.0 ); 
  glVertex2f(  bounds.x.max, bounds.y.max );
  glTexCoord2f( bounds.x.min/2.0, bounds.y.max/2.0 ); 
  glVertex2f( bounds.x.min, bounds.y.max );
  glEnd();

  glDisable(GL_TEXTURE_2D);
  glEnable(GL_BLEND);
  
  glDisable(GL_POLYGON_OFFSET_FILL );


  /*   printf( "bounds [%.2f %.2f] [%.2f %.2f] [%.2f %.2f]\n",
       bounds.x.min, bounds.x.max,
       bounds.y.min, bounds.y.max,
       bounds.z.min, bounds.z.max );
                         
  */
  
  /* simple scaling of axis labels - could be better */
  int skip = (int)( 50 / camera.scale());
  if( skip < 1 ) skip = 1;
  if( skip > 2 && skip % 2 ) skip += 1;

  //printf( "scale %.4f\n", camera.scale() );

  char str[64]; 
  PushColor( 0.2, 0.2, 0.2, 1.0 ); // pale gray

  for( double i=0; i < bounds.x.max; i+=skip)
    {      
      snprintf( str, 16, "%d", (int)i );
      Gl::draw_string(  i, 0, 0, str );
    }

  for( double i=0; i >= bounds.x.min; i-=skip)
    {
      snprintf( str, 16, "%d", (int)i );
      Gl::draw_string(  i, 0, 0, str );
    }

  
  for( double i=0; i < bounds.y.max; i+=skip)
    {
      snprintf( str, 16, "%d", (int)i );
      Gl::draw_string(  0, i, 0, str );
    }

  for( double i=0; i >= bounds.y.min; i-=skip)
    {
      snprintf( str, 16, "%d", (int)i );
      Gl::draw_string(  0, i, 0, str );
    }


  PopColor();
  
}

//draw the floor without any grid ( for robot's perspective camera model )
void Canvas::DrawFloor()
{
  bounds3d_t bounds = world->GetExtent();
        
  glEnable(GL_POLYGON_OFFSET_FILL);
  glPolygonOffset(2.0, 2.0);
        
  glColor4f( 1.0, 1.0, 1.0, 1.0 );

  glBegin(GL_QUADS);
  glVertex2f( bounds.x.min, bounds.y.min );
  glVertex2f( bounds.x.max, bounds.y.min );
  glVertex2f( bounds.x.max, bounds.y.max );
  glVertex2f( bounds.x.min, bounds.y.max );
  glEnd();
}

void Canvas::DrawBlocks() 
{
  FOR_EACH( it, models_sorted )
    (*it)->DrawBlocksTree();
}

void Canvas::DrawBoundingBoxes() 
{
  glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
  glLineWidth( 2.0 );
  glPointSize( 5.0 );
  glDisable (GL_CULL_FACE);
  
  world->DrawBoundingBoxTree();
  
  glEnable (GL_CULL_FACE);
  glLineWidth( 1.0 );
  glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
}

void Canvas::resetCamera()
{
  float max_x = 0, max_y = 0, min_x = 0, min_y = 0;
        
  //TODO take orrientation ( `a' ) and geom.pose offset into consideration
  FOR_EACH( it, world->World::children )
    {    
      Model* ptr = (*it);
      Pose pose = ptr->GetPose();
      Geom geom = ptr->GetGeom();
                
      float tmp_min_x = pose.x - geom.size.x / 2.0;
      float tmp_max_x = pose.x + geom.size.x / 2.0;
      float tmp_min_y = pose.y - geom.size.y / 2.0;
      float tmp_max_y = pose.y + geom.size.y / 2.0;
                
      if( tmp_min_x < min_x ) min_x = tmp_min_x;
      if( tmp_max_x > max_x ) max_x = tmp_max_x;
      if( tmp_min_y < min_y ) min_y = tmp_min_y;
      if( tmp_max_y > max_y ) max_y = tmp_max_y;
    }
        
  //do a complete reset
  float x = ( min_x + max_x ) / 2.0;
  float y = ( min_y + max_y ) / 2.0;
  camera.setPose( x, y );
  float scale_x = w() / (max_x - min_x) * 0.9;
  float scale_y = h() / (max_y - min_y) * 0.9;
  camera.setScale( scale_x < scale_y ? scale_x : scale_y );
        
  //TODO reset perspective cam
}


class DistFuncObj
{
  meters_t x, y;
  DistFuncObj( meters_t x, meters_t y ) 
    : x(x), y(y) {}
  
  bool operator()(const Model* a, const Model* b ) const
  { 
    const Pose a_pose = a->GetGlobalPose();
    const Pose b_pose = b->GetGlobalPose();
         
    const meters_t a_dist = hypot( y - a_pose.y, x - a_pose.x );         
    const meters_t b_dist = hypot( y - b_pose.y, x - b_pose.x );
         
    return (  a_dist < b_dist );
  }
};


void Canvas::renderFrame()
{
  //before drawing, order all models based on distance from camera
  //float x = current_camera->x();
  //float y = current_camera->y();
  //float sphi = -dtor( current_camera->yaw() );
        
  //estimate point of camera location - hard to do with orthogonal mode
  // x += -sin( sphi ) * 100;
  //y += -cos( sphi ) * 100;
        
  //double coords[2];
  //coords[0] = x;
  //coords[1] = y;
  
  // sort the list of models by inverse distance from the camera -
  // probably doesn't change too much between frames so this is
  // usually fast
  // TODO
  //models_sorted = g_list_sort_with_data( models_sorted, (GCompareDataFunc)compare_distance, coords );
  
  // TODO: understand why this doesn't work and fix it - cosmetic but important!
  //std::sort( models_sorted.begin(), models_sorted.end(), DistFuncObj(x,y) );

  glEnable( GL_DEPTH_TEST );

  if( ! showTrails )
    glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
  
  if( showOccupancy )
    ((WorldGui*)world)->DrawOccupancy();
  
  if( showVoxels )
    ((WorldGui*)world)->DrawVoxels();
  
  
  if( ! world->rt_cells.empty() )
    {
      glPushMatrix();                   
      GLfloat scale = 1.0/world->Resolution();
      glScalef( scale, scale, 1.0 ); // XX TODO - this seems slightly
                        
      world->PushColor( Color( 0,0,1,0.5) );
                        
      glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
                        
      glPointSize( 2 );
      glBegin( GL_POINTS );
                        
      for( unsigned int i=0;
           i < world->rt_cells.size();
           i++ )
        {
          char str[128];
          snprintf( str, 128, "(%d,%d)", 
                    world->rt_cells[i].x, 
                    world->rt_cells[i].y );
                                        
          Gl::draw_string(  world->rt_cells[i].x+1, 
                            world->rt_cells[i].y+1, 0.1, str );
                                        
          //printf( "x: %d y: %d\n", world->rt_regions[i].x, world->rt_regions[i].y );
          //glRectf( world->rt_cells[i].x+0.3, world->rt_cells[i].y+0.3,
          //     world->rt_cells[i].x+0.7, world->rt_cells[i].y+0.7 );
                                        
          glVertex2f( world->rt_cells[i].x, world->rt_cells[i].y );
                                        
        }
                        
      glEnd();
                        
#if 1
      world->PushColor( Color( 0,1,0,0.2) );
      glBegin( GL_LINE_STRIP );
      for( unsigned int i=0;
           i < world->rt_cells.size();
           i++ )
        {                        
          glVertex2f( world->rt_cells[i].x+0.5, world->rt_cells[i].y+0.5 );
        }
      glEnd();
      world->PopColor();
#endif
                        
      glPopMatrix();
      world->PopColor();
    }
        
  if( ! world->rt_candidate_cells.empty() )
    {
      glPushMatrix();                   
      GLfloat scale = 1.0/world->Resolution();
      glScalef( scale, scale, 1.0 ); // XX TODO - this seems slightly
                        
      world->PushColor( Color( 1,0,0,0.5) );
                        
      glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
                        
      for( unsigned int i=0;
           i < world->rt_candidate_cells.size();
           i++ )
        {
          //                     char str[128];
          //                     snprintf( str, 128, "(%d,%d)", 
          //                                              world->rt_candidate_cells[i].x, 
          //                                              world->rt_candidate_cells[i].y );
                                        
          //                     Gl::draw_string(  world->rt_candidate_cells[i].x+1, 
          //                                                                     world->rt_candidate_cells[i].y+1, 0.1, str );
                                        
          //printf( "x: %d y: %d\n", world->rt_regions[i].x, world->rt_regions[i].y );
          glRectf( world->rt_candidate_cells[i].x, world->rt_candidate_cells[i].y,
                   world->rt_candidate_cells[i].x+1, world->rt_candidate_cells[i].y+1 );
        }
                        
      world->PushColor( Color( 0,1,0,0.2) );
      glBegin( GL_LINE_STRIP );
      for( unsigned int i=0;
           i < world->rt_candidate_cells.size();
           i++ )
        {                        
          glVertex2f( world->rt_candidate_cells[i].x+0.5, world->rt_candidate_cells[i].y+0.5 );
        }
      glEnd();
      world->PopColor();
                        
      glPopMatrix();
      world->PopColor();
                        
      //world->rt_cells.clear();
    }
        
  if( showGrid )
    DrawGlobalGrid();
  else
    DrawFloor();
  
  if( showFootprints )
    {
      glDisable( GL_DEPTH_TEST ); // using alpha blending               
                
      FOR_EACH( it, models_sorted )
        (*it)->DrawTrailFootprint();
                
      glEnable( GL_DEPTH_TEST );
    }
  
  if( showFlags ) 
    {
      glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
      glBegin( GL_TRIANGLES );

      FOR_EACH( it, models_sorted )
        (*it)->DrawFlagList();

      glEnd();
    }

  if( showTrailArrows )
    FOR_EACH( it, models_sorted )
      (*it)->DrawTrailArrows();  
  
  if( showTrailRise )
    FOR_EACH( it, models_sorted )
      (*it)->DrawTrailBlocks();  
    
  if( showBlocks )
    DrawBlocks();
        
  if( showBBoxes )
    DrawBoundingBoxes();
        
  // TODO - finish this properly
  //LISTMETHOD( models_sorted, Model*, DrawWaypoints );

  FOR_EACH( it, selected_models )
    (*it)->DrawSelected();

  // useful debug - puts a point at the origin of each model
  //for( GList* it = world->World::children; it; it=it->next ) 
  // ((Model*)it->data)->DrawOriginTree();
  
  // draw the model-specific visualizations
  if( world->sim_time > 0 )
    {
      if( showData ) {
        if ( ! visualizeAll ) {
          FOR_EACH( it, world->World::children )
            (*it)->DataVisualizeTree( current_camera );
        }
        else if ( selected_models.size() > 0 ) {
          FOR_EACH( it, selected_models )
            (*it)->DataVisualizeTree( current_camera );
        }
        else if ( last_selection ) {
          last_selection->DataVisualizeTree( current_camera );
        }
      }
    }

  if( showGrid ) 
    FOR_EACH( it, models_sorted )
      (*it)->DrawGrid();
                  
  if( showStatus ) 
    {
      glPushMatrix();
      //ensure two icons can't be in the exact same plane
      if( camera.pitch() == 0 && !pCamOn )
        glTranslatef( 0, 0, 0.1 );
                
      FOR_EACH( it, models_sorted )
        (*it)->DrawStatusTree( &camera );
                
      glPopMatrix();
    }
  
  if( world->ray_list.size() > 0 )
    {
      glDisable( GL_DEPTH_TEST );
      PushColor( 0,0,0,0.5 );
      FOR_EACH( it, world->ray_list )
        {
          float* pts = *it;
          glBegin( GL_LINES );
          glVertex2f( pts[0], pts[1] );
          glVertex2f( pts[2], pts[3] );
          glEnd();
        }  
      PopColor();
      glEnable( GL_DEPTH_TEST );
                 
      world->ClearRays();
    } 
        
  if( showClock )
    {           
      glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
                
      //use orthogonal projeciton without any zoom
      glMatrixMode (GL_PROJECTION);
      glPushMatrix(); //save old projection
      glLoadIdentity ();
      glOrtho( 0, w(), 0, h(), -100, 100 );     
      glMatrixMode (GL_MODELVIEW);

      glPushMatrix();
      glLoadIdentity();
      glDisable( GL_DEPTH_TEST );

      std::string clockstr = world->ClockString();
      if( showFollow == true && last_selection )
        clockstr.append( " [FOLLOW MODE]" );
                
      float txtWidth = gl_width( clockstr.c_str());
      if( txtWidth < 200 ) txtWidth = 200;
      int txtHeight = gl_height();
                
      const int margin = 5;
      int width, height;                
      width = txtWidth + 2 * margin;
      height = txtHeight + 2 * margin; 
                
      // TIME BOX
      colorstack.Push( 0.8,0.8,1.0 ); // pale blue
      glRectf( 0, 0, width, height );
      colorstack.Push( 0,0,0 ); // black
      Gl::draw_string( margin, margin, 0, clockstr.c_str() );
      colorstack.Pop();
      colorstack.Pop();
                
      // ENERGY BOX
      if( PowerPack::global_capacity > 0 )
        {
          colorstack.Push( 0.8,1.0,0.8,0.85 ); // pale green
          glRectf( 0, height, width, 90 );
          colorstack.Push( 0,0,0 ); // black
          Gl::draw_string_multiline( margin, height + margin, width, 50, 
                                     world->EnergyString().c_str(), 
                                     (Fl_Align)( FL_ALIGN_LEFT | FL_ALIGN_BOTTOM) );     
          colorstack.Pop();
          colorstack.Pop();
        }
                         
      glEnable( GL_DEPTH_TEST );
      glPopMatrix();

      //restore camera projection
      glMatrixMode (GL_PROJECTION);
      glPopMatrix();
      glMatrixMode (GL_MODELVIEW);
    }

  if( showScreenshots && (frames_rendered_count % screenshot_frame_skip == 0) )
    Screenshot();

  frames_rendered_count++; 
}


void Canvas::EnterScreenCS()
{
  //use orthogonal projection without any zoom
  glMatrixMode (GL_PROJECTION);
  glPushMatrix(); //save old projection
  glLoadIdentity ();
  glOrtho( 0, w(), 0, h(), -100, 100 ); 
  glMatrixMode (GL_MODELVIEW);
  
  glPushMatrix();
  glLoadIdentity();
  glDisable( GL_DEPTH_TEST );
}

void Canvas::LeaveScreenCS()
{
  glEnable( GL_DEPTH_TEST );
  glPopMatrix();
  glMatrixMode (GL_PROJECTION);
  glPopMatrix();
  glMatrixMode (GL_MODELVIEW);
}


void Canvas::Screenshot()
{

  int width = w();
  int height = h();
  int depth = 4; // RGBA

  // we use RGBA throughout, though we only need RGB, as the 4-byte
  // pixels avoid a nasty word-alignment problem when indexing into
  // the pixel array.

  // might save a bit of time with a static var as the image size rarely changes
  static std::vector<uint8_t> pixels;
  pixels.resize( width * height * depth );
  
  glFlush(); // make sure the drawing is done
  // read the pixels from the screen
  glReadPixels( 0,0, width, height, GL_RGBA, GL_UNSIGNED_BYTE, &pixels[0] );                     
  
  static uint32_t count = 0;             
  char filename[64];
  snprintf( filename, 63, "stage-%06d.png", count++ );
  
  FILE *fp = fopen( filename, "wb" );
  if( fp == NULL ) 
    {
      PRINT_ERR1( "Unable to open %s", filename );
    }
  
  // create PNG data
  png_structp pp = png_create_write_struct(PNG_LIBPNG_VER_STRING, 0, 0, 0);
  assert(pp);
  png_infop info = png_create_info_struct(pp);
  assert(info);

  // setup the output file
  png_init_io(pp, fp);

  // need to invert the image as GL and PNG disagree on the row order
  png_bytep rowpointers[height];
  for( int i=0; i<height; i++ )
    rowpointers[i] = &pixels[ (height-1-i) * width * depth ];

  png_set_rows( pp, info, rowpointers ); 

  png_set_IHDR( pp, info, 
                width, height, 8, 
                PNG_COLOR_TYPE_RGBA, 
                PNG_INTERLACE_NONE, 
                PNG_COMPRESSION_TYPE_DEFAULT, 
                PNG_FILTER_TYPE_DEFAULT);

  png_write_png( pp, info, PNG_TRANSFORM_IDENTITY, NULL );

  // free the PNG data - we reuse the pixel array next call.
  png_destroy_write_struct(&pp, &info);
  
  fclose(fp);
  
  printf( "Saved %s\n", filename );
}

void Canvas::perspectiveCb( Fl_Widget* w, void* p ) 
{
  Canvas* canvas = static_cast<Canvas*>( w );
  Option* opt = static_cast<Option*>( p ); // pCamOn
  if ( opt ) {
    // Perspective mode is on, change camera
    canvas->current_camera = &canvas->perspective_camera;
  }
  else {
    canvas->current_camera = &canvas->camera;
  }
        
  canvas->invalidate();
}

void Canvas::createMenuItems( Fl_Menu_Bar* menu, std::string path )
{
  showData.createMenuItem( menu, path );
  //  visualizeAll.createMenuItem( menu, path );
  showBlocks.createMenuItem( menu, path );
  showFlags.createMenuItem( menu, path );
  showClock.createMenuItem( menu, path );
  showFlags.createMenuItem( menu, path );
  showFollow.createMenuItem( menu, path );
  showFootprints.createMenuItem( menu, path );
  showGrid.createMenuItem( menu, path );
  showStatus.createMenuItem( menu, path );
  pCamOn.createMenuItem( menu, path );
  pCamOn.menuCallback( perspectiveCb, this );
  showOccupancy.createMenuItem( menu, path );
  showTrailArrows.createMenuItem( menu, path );
  showTrails.createMenuItem( menu, path ); 
  showTrailRise.createMenuItem( menu, path );  // broken
  showBBoxes.createMenuItem( menu, path );
  //showVoxels.createMenuItem( menu, path );  
  showScreenshots.createMenuItem( menu, path );  
}


void Canvas::Load( Worldfile* wf, int sec )
{
  this->wf = wf;
  camera.Load( wf, sec );
  perspective_camera.Load( wf, sec );           
        
  interval = wf->ReadInt(sec, "interval", interval );

  screenshot_frame_skip = wf->ReadInt( sec, "screenshot_skip", screenshot_frame_skip );
  if( screenshot_frame_skip < 1 )
    screenshot_frame_skip = 1; // avoids div-by-zero if poorly set

  showData.Load( wf, sec );
  showFlags.Load( wf, sec );
  showBlocks.Load( wf, sec );
  showBBoxes.Load( wf, sec );
  showBlur.Load( wf, sec );
  showClock.Load( wf, sec );
  showFollow.Load( wf, sec );
  showFootprints.Load( wf, sec );
  showGrid.Load( wf, sec );
  showOccupancy.Load( wf, sec );
  showTrailArrows.Load( wf, sec );
  showTrailRise.Load( wf, sec );
  showTrails.Load( wf, sec );
  //showVoxels.Load( wf, sec );
  showScreenshots.Load( wf, sec );
  pCamOn.Load( wf, sec );

  if( ! world->paused )
    // // start the timer that causes regular redraws
    Fl::add_timeout( ((double)interval/1000), 
                     (Fl_Timeout_Handler)Canvas::TimerCallback, 
                     this);

  invalidate(); // we probably changed something
}

void Canvas::Save( Worldfile* wf, int sec )
{
  camera.Save( wf, sec );
  perspective_camera.Save( wf, sec );   
        
  wf->WriteInt(sec, "interval", interval );

  showData.Save( wf, sec );
  showBlocks.Save( wf, sec );
  showBBoxes.Save( wf, sec );
  showBlur.Save( wf, sec );
  showClock.Save( wf, sec );
  showFlags.Save( wf, sec );
  showFollow.Save( wf, sec );
  showFootprints.Save( wf, sec );
  showGrid.Save( wf, sec );
  showOccupancy.Save( wf, sec );
  showTrailArrows.Save( wf, sec );
  showTrailRise.Save( wf, sec );
  showTrails.Save( wf, sec );
  showVoxels.Save( wf, sec );
  showScreenshots.Save( wf, sec );
  pCamOn.Save( wf, sec );
}


void Canvas::draw()
{
  //Enable the following to debug camera model
  //    if( loaded_texture == true && pCamOn == true )
  //            return;
  
  if (!valid() ) 
    { 
      if( ! init_done )
        InitGl();
      if( ! texture_load_done )
        InitTextures();
                
      if( pCamOn == true ) 
        {
          perspective_camera.setAspect( static_cast< float >( w() ) / static_cast< float >( h() ) );
          perspective_camera.SetProjection();
          current_camera = &perspective_camera;
        } 
      else 
        {
          bounds3d_t extent = world->GetExtent();
          camera.SetProjection( w(), h(), extent.y.min, extent.y.max );
          current_camera = &camera;
        }
                
      glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
    }            

  //Follow the selected robot   
  if( showFollow  && last_selection ) 
    {
      Pose gpose = last_selection->GetGlobalPose();
      if( pCamOn == true )
        {
          perspective_camera.setPose( gpose.x, gpose.y, 0.2 );
          perspective_camera.setYaw( rtod( gpose.a ) - 90.0 );
        } 
      else 
        {
          camera.setPose( gpose.x, gpose.y );
        }
    }
  
  current_camera->Draw();       
  renderFrame();
}

void Canvas::resize(int X,int Y,int W,int H) 
{
  Fl_Gl_Window::resize(X,Y,W,H);

  if( ! init_done ) // hack - should capture a create event to do this rather thann have it in multiple places
    InitGl();
        
  FixViewport(W,H);
  invalidate();
}